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ABSTRACT 


Autonomous  operation  of  a  small  rigid  hull  inflatable  boat  (RHIB)  is  a  complex  problem 
that  requires  a  robust  network  of  sensors,  controllers,  processors,  and  actuators. 
Furthermore,  autonomous  navigation  requires  accurate  state  estimation,  fusing  and 
filtering  data  from  an  array  of  sensors  to  give  the  best  possible  estimates  of  attitude, 
position,  and  velocity. 

This  thesis  will  address  the  hardware  modifications  and  navigation  state 
estimators  used  to  configure  the  SeaFox  Mk  II  RHIB  for  future  autonomous  operations. 
The  study  began  with  a  RHIB  capable  of  manual  and  remote-controlled  operation.  The 
proprietary  controllers  and  processors  were  replaced  with  an  open  architecture  system 
that  enabled  an  autonomous  mode  of  operation  and  data  collection  from  a  suite  of  global 
positioning  satellite  receivers  and  inertial  measurement  units.  Multiple  navigation  state 
estimators  were  designed  using  the  extended  Kalman  filter  and  several  variants  of  the 
unscented  Kalman  filter.  Each  filter  was  evaluated  against  simulated  and  actual  sea  trial 
data  to  determine  its  accuracy,  robustness,  and  computational  efficiency. 
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I.  INTRODUCTION 


A,  BACKGROUND 

The  United  States  Navy’s  Unmanned  Surface  Vehicle  Master  Plan  defines  the 
Navy’s  long  term  commitment  to  the  research  and  development  of  unmanned  surface 
vehicles.  There  are  seven  critical  mission  areas  the  Navy  intends  to  support  with 
unmanned  surface  craft:  mine  countermeasures  (MCM),  anti-submarine  warfare  (ASW), 
maritime  security  (MS),  surface  warfare  (SUW),  Special  Operations  Forces  (SOF) 
support,  electronic  warfare  (EW),  and  maritime  interdiction  operations  (MIO)  support 
[1].  Each  warfare  mission  area  requires  unique  system  configurations  and  demands,  but 
share  common  ties  to  the  problem  of  open  water  navigation  and  obstruction  avoidance. 

The  Naval  Postgraduate  School  Center  for  Autonomous  Vehicle  Research 
(CAVR)  has  been  developing  surface  and  subsurface  vehicles  to  navigate  unknown 
waterways  with  semi  and  full  autonomy  with  aims  to  support  SOE  and  intelligence, 
surveillance,  and  reconnaissance  (ISR)  missions.  These  vessels  use  various  sensors  to 
collect  data  from  the  surrounding  environment  to  make  navigation  decisions  and  avoid 
surface  and  subsurface  obstacles. 

The  SEAEOX  I  (Eigure  1),  built  by  Northwind  Marine,  Inc.  and  modified  by 
previous  thesis  students  and  faculty,  was  CAVR’s  first  attempt  at  full  surface  vessel 
autonomy.  SEAEOX  I  has  been  used  to  conduct  experiments  for  autonomous  riverine 
navigation,  networked  collaborative  multi-vehicle  operations,  and  maritime  interdiction 
operations.  While  the  vessel  has  seen  much  success,  it  suffers  from  significant  control 
command  to  execution  lag  due  to  legacy  and  proprietary  controller  interfaces.  This  thesis 
and  future  work  will  focus  on  building  upon  the  successes  of  the  SEAEOX  I  while 
leveraging  new  technologies  to  avoid  previous  control  limitations  on  a  new  platform  for 
autonomous  surface  vessel  (ASV)  research. 
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Figure  1 .  SEAFOX I  in  low  speed  (left)  and  high  speed  (right)  operation,  from  [2], 


B.  SEAFOX  II 

The  SEAFOX  II  (see  Figure  2),  designed  and  built  by  Northwind  Marine,  Ine.,  is 
a  5.1  meter  rigid  hull  inflatable  boat  (RHIB).  Its  water  jet  propulsion  system  is  powered 
by  a  JP-5  fueled  Mercury  3.0  liter  V-6  OptiMax  JP  racing  engine  capable  of  sustained 
speeds  over  30  knots.  The  water  jet  acts  as  both  propulsion  and  rudder  giving  the  vessel 
good  performance  at  high  speed  but  poor  maneuverability  at  low  speed. 


Figure  2.  SEAFOX  II  prior  to  modification. 
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Northwind  Marine  designed  the  SEAFOX  II  for  two  modes  of  operation;  manual 
and  remote.  In  manual  operation  mode  the  boat  is  controlled  by  an  onboard  operator 
through  the  helm  and  throttle  lever  or  a  digital  autopilot  control  display.  For  remote 
operation  the  SEAFOX  II  is  outfitted  with  a  wireless  communications  suite  that  relays 
commands  and  data  between  the  vessel  and  a  remote  control  station.  In  both  manual 
autopilot  and  remote  control  operation,  all  control  signals  are  routed  through  a 
proprietary  signal  processor  unit  (SPU)  for  relay  to  the  rudder  and  throttle  actuators. 
Figure  3  depicts  the  legacy  SEAFOX  II  architecture  from  control  input  to  output.  Prior  to 
modification,  the  SEAFOX  II  provided  no  organic  means  of  capturing  rudder  feedback, 
throttle  position,  engine  RPM,  GPS  location  or  speed  for  online  or  offline  data  analysis. 
The  SPU  and  associated  controllers  were  locked  in  a  proprietary  system  leaving  few 
options  for  experimental  methods  of  control. 


Figure  3.  Fegacy  SEAFOX  II  control  input  architecture. 


As  the  SEAFOX  II  held  little  capability  for  autonomous  control  experimentation, 
it  was  used  primarily  as  a  designated  chase  boat  for  the  SEAFOX  I  ASV  as  well  as  a 
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launch/recovery  platform  for  Hy droid  REMUS  100  autonomous  underwater  vehicles 
(AUVs). 


C.  DESIGN  REQUIREMENTS 

In  support  of  ongoing  research  programs,  the  SEAFOX  II  is  being  modified  to 
deploy  the  autonomous  topographic  large  area  survey  (ATLAS)  sonar  system  designed 
by  the  Applied  Research  Laboratories  of  the  University  of  Texas  at  Austin  (ARL:UT). 
The  ATLAS  sonar  will  be  mounted  to  the  bow  via  an  electrically-actuated  truss  designed 
by  Hullux  Subsea  Technologies  to  deploy  the  sonar  below  the  waterline,  as  shown  in 
Figure  4. 


Figure  4.  Mounting  and  operation  of  the  ATLAS  Sonar 

The  ATLAS  is  designed  to  detect  subsurface  obstacles,  particularly  in  the  riverine 
and  littoral  environment.  Aside  from  obstacle  detection,  the  sonar  will  provide  local 
bathymetry  data  such  as  water  depth,  temperature,  and  sound  speed  velocity.  In  order  for 
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the  sonar  to  provide  aeeurate  information  it  requires  the  following  data  inputs  at  an 
update  rate  of  10  Hz: 

•  Platform  navigation  time:  The  referenee  time  of  the  vehiele  for  all 
parameters  in  the  eurrent  navigation  data  sample. 

•  Geographie  latitude  and  longitude. 

•  Piteh  angle:  Positive  angles  are  measured  upward  from  the  horizontal 
plane. 

•  Roll  angle:  Positive  angles  are  measured  eloekwise  when  viewed  from  the 
platform  bow. 

•  Heading  angle:  Positive  angles  are  measured  eloekwise  from  true  north. 

•  North  and  east  veloeity  eomponents. 

•  Platform  water  speed:  Speed  of  the  platform  relative  to  the  water  in  the 
direetion  of  the  eurrent  heading. 

Beyond  providing  quality  navigation  data  to  the  ATLAS  sonar,  further 
modifieations  needed  to  be  made  to  the  existing  eontrol  network  to  advanee  the  progress 
towards  full  autonomy.  To  allow  for  a  more  eomplete  researeh  vessel  and  to  eireumvent 
the  existing  proprietary  eontroller  network,  the  SEAT OX  II  needed  to  be  outfitted  with  a 
new  eontrol  arehiteeture  and  sensor  suite  eapable  of  the  following: 

•  An  open  arehiteeture  network  eapable  of  seheduling  tasks,  exeeuting 
orders,  and  logging  data  from  multiple  eontrollers,  aetuators,  and  sensors. 

•  Three  modes  of  operational  eontrol:  manual,  remote  eontrolled,  and  fully 
autonomous. 

•  Inertial  and  GPS  data. 

•  Engine  eontrol  and  feedbaek. 

•  Rudder  eontrol  and  feedbaek. 

•  Wireless  eommunieations  for  remote  eontrol  operation  and  monitoring. 

D.  THESIS  OBJECTIVES 

To  meet  the  design  requirements,  this  thesis  foeused  on  two  major  objeetives: 
first,  to  modify  the  SEAT OX  II  hardware  to  provide  the  ability  to  eapture  and  store  live 
GPS  and  inertial  measurement  data,  and  seeondly  to  investigate  methods  of  determining 
the  vessel’s  attitude,  veloeity,  and  position  at  a  high  update  rate  in  support  of  both  sonar 
and  future  autonomous  operations.  In  pursuit  of  the  seeond  objeetive  a  navigation  state 
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estimator  needed  to  be  designed.  For  this  task,  five  Kalman  filter  variants  for  state 
estimation  were  investigated  for  aeouraey,  robustness,  and  eomputational  effieieney:  the 
extended  Kalman  filter  (EKF),  unseented  Kalman  filter  (UKF),  spherieal  simplex 
unseented  Kalman  filter  (SSUKF),  square  root  unseented  Kalman  filter  (SR-UKF),  and 
the  square  root  spherieal  simplex  unseented  Kalman  filter  (SR-SSUKF).  Eaeh  estimator 
was  evaluated  using  simulated  data  and  the  best  performing  estimator  was  evaluated 
against  data  obtained  from  the  SEAEOX II  while  operating  on  the  Monterey  Bay. 
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II.  SEAFOX  MODIFICATIONS 


A,  OVERVIEW 

Building  a  sensor  and  controller  network  for  an  autonomous  vehicle  from 
scratch  has  potential  to  be  a  lengthy  and  costly  process  depending  on  the  state  of  the 
baseline  vehicle.  The  SEAFOX  II  provided  a  unique  platform  that  was  originally 
designed  from  the  ground  up  with  unmanned  operation  in  mind.  All  of  the  actuators 
necessary  to  operate  the  rudder  and  throttle  were  already  integrated  into  the  baseline 
system.  While  this  thesis  focused  on  modifications  specific  to  the  SEAFOX  II,  the 
implemented  system  architecture  has  been  demonstrated  to  work  on  a  variety  of  vessels. 
The  simplicity  of  this  architecture  enabled  its  complete  installation  onboard  SEAFOX  II 
in  under  one  week. 

B,  SEABORNE  CONTROLLER  AREA  NETWORK 

The  seaborne  controller  area  network  (SEACAN)  was  developed  by  a  joint 
government  and  contractor  team  managed  by  the  Surface  Targets  Branch,  Naval  Air 
Warfare  Center  Weapons  Division  (NAWCWD)  at  Naval  Base  Ventura  County,  Port 
Hueneme,  California.  It  is  a  scalable,  open  architecture,  root  level  access  system  that 
permitted  CAVR  to  quickly  upgrade  the  the  SEAFOX  II  autonomous  capabilities. 

The  Surface  Targets  Group  develops,  builds,  and  maintains  a  growing  fleet  of 
unmanned  surface  vessel  targets,  two  examples  of  which  are  shown  in  Figure  5.  The 
vessel  by  which  the  SEAFOX  II  modifications  based  is  the  high  speed  maneuverable 
surface  target  (HSMST),  a  7  meter,  twin  outboard  USV  with  a  top  speed  of  45  knots  [3]. 
The  HSMST  is  remote  operated  from  a  base  station  and  is  capable  of  autopilot  waypoint 
tracking.  The  HSMST  class  of  vessel  was  designed  and  built  with  ease  of  replication,  low 
cost,  and  robustness  in  mind  as  these  targets  are  often  destroyed  by  the  naval  weapon 
systems  that  engage  them. 
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Figure  5.  High  speed  maneuverable  surfaee  target  (top),  ship  deployable  surfaee 

target — Jet  Ski  (bottom),  from  [3]. 

The  controller  area  network  (CAN)  is  the  automotive  serial  bus  standard  for  intra- 
vehicular  device  communication,  connecting  a  network  of  nodes  that  operate  sensors, 
actuators,  and  other  control  devices  without  the  need  of  a  central  host  computer.  The 
SEACAN  system  is  based  on  the  architecture  of  the  CAN,  adapted  for  use  in  the 
maritime  environment.  The  nodes  the  make  up  the  SEACAN  system  are  listed  in  Table  1. 
Each  node,  with  the  exception  of  the  king  and  port  and  starboard  engine  actuator  nodes. 
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contains  a  central  processing  unit,  4th  generation  (CPU-4)  microcontroller  board  for 
sensor  and  actuator  control. 


Node  List 

• 

King 

• 

Rudder  Feedback 

• 

Rudder  Controller 

• 

Compass  &  Heading 

• 

RF  Modem 

• 

GPS 

• 

Engine  Controller  (Port  &  Starboard) 

• 

Throttle  Controller  (Port  &  Starboard) 

Table  1.  List  of  nodes  installed  on  SEAT OX  II. 


The  king  node  provides  the  monitoring  and  supervision  of  the  vessel’s  controller 
area  network  (CAN)  bus  activity  and  indicates  system  status  via  an  LCD  display  as 
shown  in  Figure  6.  The  display  not  only  shows  node  status  such  as  heading  or  rudder 
positions  but  also  has  various  options  for  tuning  the  system  such  as  resetting  the  rudder’s 
centerline  zero  position. 


Figure  6.  King  node  system  status  display. 
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The  remainder  of  the  nodes  listed  in  Table  1  function  as  their  name  implies. 
Several  nodes  are  depicted  in  Figure  7.  The  rudder  controller  regulates  the  single  speed 
hydraulic  steering  pump  with  pulse  width  modulation  to  allow  for  variable  motor  speeds. 
The  rudder  controller  utilizes  a  tuned  proportional-integral-derivative  controller  with  set 
saturation  limits  to  insure  smooth  rudder  movements.  The  rudder  feedback  provides 
current  rudder  angle  information.  The  heading  node  relays  Euler  angles  and  rates 
provided  by  the  MicroStrain  3DM-GX1  inertial  measurement  unit  (IMU).  When 
operating  in  remote  mode,  the  RF  modem  node  sends  periodic  information  to  the  base 
station  operator  and  receives  commands  as  necessary.  For  safety  purposes,  if  the  carrier 
signal  is  lost  for  a  specified  amount  of  time,  the  vessel  will  enter  a  holding  pattern  while 
awaiting  reconnection.  If  reconnection  with  the  base  station  is  not  regained  within  a 
specified  time  window  the  kill  switch  is  enabled  and  the  engines  shutdown.  The  engine 
controller  nodes  monitor  the  engine  RPM  and  control  the  engine  startup  and  shutdown 
while  the  throttle  controller  monitors  the  throttle  position  and  executes  throttle  position 
commands. 


Rudder 

Feedback 


Engine  & 
Throttle 
Controller 


Heading 

Node 


GPS  and 
Comms  Antenna 


3-in-1 

IMU 


Figure  7.  Several  of  the  nodes  and  sensors  installed  on  the  SEAFOX  II. 
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c. 


ADDITIONAL  SENSORS 


In  addition  to  the  sensors  organie  to  the  SEACAN  system,  an  additional  high 
performanee  IMU  and  GPS  reeeiver  were  installed.  The  IMU  installed  is  a  Honeywell 
HG1700AG58  containing  a  three-axis  ring  laser  gyro  (REG)  and  accelerometer.  The 
IMU  sensor  specifications  are  listed  in  Table  2.  The  IMU  takes  readings  at  600  Hz  and 
outputs  to  a  standard  RS-422  serial  interface  at  a  rate  of  100  Hz.  The  additional  GPS 
receiver,  a  ComNav  Vector  G2  GPS  Satellite  Compass,  shown  as  DGPS  in  Eigure  7, 
houses  two  GPS  receivers  0.5  meters  apart  as  well  as  a  two-axis  gyro.  The  DGPS  outputs 
position,  heading,  turn  rate,  pitch  or  roll,  course  over  ground,  and  speed  over  ground  at  a 
rate  of  20  Hz.  The  ComNav  GPS  has  5  meter  position  accuracy  and  0.5  degree  heading 
accuracy  [4]. 


Honeywel 

11 HG1700AG50  Sensor  Specifical 

ions 

Sensor 

Operating 

Range 

Scale 

Factor 

Scale 

Factor 

Accnracy 

Bias 

Axis 

Alignment 

Ontpnt 

Noise 

Freqnency 

Response 

Accel. 

37  (+/-  g’s) 

2‘-N 

600 

(ft/sec^* 

1000  (ppm, 

1  sigma) 

50  (milli- 
g’s,  1 
sigma) 

0.5  (milli- 
rad,  1 
sigma) 

100 

(milli-g’s 

max) 

70  max.  (90 
deg  phase  lag 
at  freq  in  Hz) 

Gyro. 

1074  (+/- 
deg/sec) 

600 

(rad/sec) 

1000  (ppm, 

1  sigma) 

50 

(deg/hr,  1 
sigma) 

0.5  (milli- 
rad,  1 
sigma) 

45  (milli- 
rads/sec 
max) 

70  max.  (90 
deg  phase  lag 
at  freq  in  Hz) 

Table  2.  Honeywell  HG1700AG58  sensor  specifications,  from  [5]. 


D,  ROBOT  OPERATING  SYSTEM 

An  added  benefit  to  the  SEACAN  structure  is  the  ability  to  issue  rudder  and 

engine  commands  from  outside  the  system.  This  plug  and  play  capability  allows  for  the 

development  of  an  autonomous  mode  of  operation,  controlled  by  an  onboard  computer. 

The  Robot  Operating  System  (ROS)  was  used  to  develop  the  software  to  enable  data 
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acquisition  and  of  the  SEAFOX  II.  ROS  is  a  widely  supported,  open  souree,  open 
arehiteeture  operating  system  that  provides  a  large  library  of  tools  assoeiated  with  robot 
eontrol  and  oommunieation  [6].  ROS  runs  on  SEAFOX  II  from  an  embedded  PC- 104  and 
exeeutes  programs  that  store  sensor  data  from  the  SEACAN  system  and  other  sensors  in  a 
eommon  database,  run  the  designated  navigation  filters,  and  issue  eommands  to  the 
SEACAN  rudder  and  throttle  eontrollers.  Onee  the  ATE  AS  sonar  suite  is  installed, 
programs  that  analyze  sonar  data,  generate  navigation  paths,  and  follow  those  paths  will 
be  ereated  to  enable  a  fully  autonomous  mode  of  operation. 
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III.  KALMAN  FILTER  ESTIMATION  REVIEW 


A,  OVERVIEW 

The  Kalman  filter,  developed  by  [7],  is  a  powerful  estimator  used  in  a  wide  range 
of  applications  from  computer  vision  to  navigation.  For  linear  systems,  the  Kalman  filter 
has  been  proven  to  provide  an  optimal  solution  to  minimize  the  error  covariance  between 
the  true  state  and  its  estimate  [8].  For  non-linear  systems,  the  extended  Kalman  filter 
(EKF)  is  a  commonly  used  method  for  state  estimation,  but  provides  sub-optimal  error 
covariance  minimization  due  to  its  first  order  linearization  of  non-linear  processes.  Many 
alternatives  to  the  classic  EKF  have  been  developed  that  vary  in  computational 
complexity  and  cost.  In  this  paper,  the  unscented  Kalman  filter  (ElKF)  and  several 
variants  are  explored  as  an  alternative  to  the  EKF  in  order  to  make  use  of  the  highly 
nonlinear  measurement  equations  used  in  the  subsequent  state  estimator  design. 

B,  EXTENDED  KALMAN  FILTER 

The  extended  Kalman  filter  has  been  studied  in  depth  with  numerous  publications 
available  deriving  and  analyzing  the  process.  This  section  uses  the  equations  derived  in 
[9]  and  chapters  4  and  5  of  [8]  to  summarize  the  EKF  process. 

The  discrete  time  extended  Kalman  filter  addresses  the  issue  of  estimating  the  true 
state  X  e  i?”  driven  by  the  nonlinear  process 

(3.1) 

where  Uk  is  a  process  input  measurement  and  Vk  is  process  noise  at  time  instance  k. 

The  process  nonlinear  measurements  z  &  R"‘  are  defined  by 

z^^h{x„w^)  (3.2) 

where  Wk  is  the  measurement  noise  at  time  instance  k. 

The  process  and  measurement  noise  is  assumed  to  be  zero  mean  Gaussian, 
independent,  and  uncorrelated  white  noise: 
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(3.3) 


v, ~7V(O,0) 

w, -N{0,R) 

The  estimate  of  the  state  at  time  k  is  shown  as  .  The  nonlinear  state  time 
propagation  equation  and  measurement  process  are  defined  as 


^k=h{x„0) 

which  are  approximations  of  the  state  and  measurement  with  noise  terms  set  to  their 
expected  value  of  zero. 

By  linearizing  the  non-linear  functions  in  Equation  (3.4)  around  the  current  value 
of  the  state  estimate,  the  Kalman  filter  solution  becomes 

Xk^i~x,+F,{x,-x,)  +  V,v, 

^k~^k+fikiXk-x,)  +  W,w, 

where 


F-K 

"  dx, 


(ii.Mi.O) 


"  fx, 


(v.O) 


tv 


(4.0) 


(3.6) 


are  the  first  order  Jacobians  of  the  nonlinear  state  process,  state  excitation  process, 
measurement  process,  and  measurement  noise  process,  respectively. 

By  defining  an  error  process  as  the  difference  between  the  true  and  estimated 

values 


with  an  associated  covariance  of 


P,-N[^,E[eX'\) 


(3.7) 


(3.8) 
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where  £'[•]  is  the  expected  value  operator. 

The  process  and  measurement  noise  processes  are  now  defined  as 

a~A'(o.F.eF/) 

R,~N(0,ir,RWl} 

Equation  (3.5)  can  then  be  substituted  into  Equation  (3.7)  to  form 

making  the  error  process  linear  with  respect  to  the  state  error. 


(3.10) 


Since  the  actual  measurement  at  time  k  is  known,  it  is  possible  to  use  the 
measurement  error  to  estimate  the  predicted  state  error  and  correct  an  a  posteriori 


state  estimate 


Xk=x,  + 


(3.11) 


The  Kalman  gain  minimizes  the  error  covariance  The  Kalman  gain  is 
calculated  at  each  time  instance  by  the  equation 


k,=p,-h,{h,p;hJ  +  r,)'' 

where  the  superscript  denotes  an  a  priori  estimate. 

The  a  posteriori  state  estimate  is  rewritten  to  take  the  form 

where  the  “+”  superscript  denotes  the  corrected  a  posteriori  state  estimate. 
The  error  state  covariance  is  then  updated  as 

p:={i-k,h,)p- 


(3.12) 


(3.13) 


(3.14) 


The  state  and  error  covariance  estimates  are  projected  ahead  to  the  next  time 
instance  by 


(3.15) 
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(3.16) 

The  EKF  recursion  is  executed  at  every  time  instance  as  shown  in  Figure  8.  The 
Kalman  gain  is  computed  with  the  time  updates  from  the  previous  time  step.  The  a  priori 
state  estimate  and  error  state  is  then  corrected  providing  the  a  posteriori  estimates  which 
are  propagated  through  their  nonlinear  transformations  to  provide  a  time  update. 


Figure  8.  EKF  recursion  summary. 


The  performance  of  the  EKF  is  tightly  coupled  to  the  nonlinearity  of  the  system 

process  and  measurements.  The  EKF  state  distribution  is  approximated  by  a  Gaussian 

random  variable  which  is  propagated  through  a  first-order  linearization  of  the  nonlinear 

process.  If  the  error  propagation  cannot  be  accurately  approximated  with  a  linearized 

function  due  to  the  function’s  higher  order  terms,  then  the  estimates  will  be  poor  and  may 

diverge.  An  additional  source  of  error  is  the  difficulty  of  developing  and  coding  the 

Jacobian  matrices  from  Equation  (3.6)  which  can  be  highly  complex  and  extensive 

depending  on  the  nonlinear  process  and  measurement  functions.  Fligher  order  filtering 

schemes  such  as  Bayesian  or  Particle  filters  address  the  issues  associated  with 

linearization  by  approximating  the  true  probability  density  function  with  large  numbers 
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of  random  sample  points,  but  at  a  significant  computational  cost.  For  further  analysis  of 
the  Bayesian  filter  see  [10].  As  the  state  estimators  developed  for  this  thesis  are  intended 
for  testing  in  a  real-time  system,  a  less  computationally  burdensome  filter  is  required. 

C.  UNSCENTED  KALMAN  FILTER 

The  unscented  Kalman  Filter  (UKF),  first  published  in  [11],  is  founded  on  the 
idea  that  it  is  simpler  to  approximate  a  probability  distribution  function  than  an 
arbitrary  nonlinear  function.  By  propagating  a  finite  set  of  specifically  chosen  points 
(sigma  points),  with  a  given  mean  and  covariance,  through  a  nonlinear  function  to  form 
a  cloud  of  transformed  points,  the  statistics  of  the  transformed  points  can  be  estimated 
to  approximate  the  true  mean  and  covariance  as  depicted  in  Figure  9.  For  Gaussian 
inputs,  the  UKF  approach  is  accurate  to  the  third  order  Taylor  series  expansion  of  a 
nonlinear  function,  and  for  non-Gaussian  inputs  the  approximations  are  accurate  to  the 
second  order,  whereas  the  linearization  approach  of  the  EKF  is  only  accurate  to  the  first 
order  [12]. 


Figure  9.  The  unscented  transformation  of  an  arbitrary  nonlinear  function,  after  [13]. 
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Several  variants  of  unseented  transformations  have  been  developed  for  the 
purpose  of  eomputational  savings  sueh  as  the  square  root  UKF  (SR-UKF)  [14],  spherieal 
simplex  UKF  (SSUKF)  [15],  and  the  square  root  spherieal  simplex  UKF  (SR-SSUKF) 
[16].  The  algorithms  for  each  variant,  including  the  standard  UKF  implementation  will  be 
developed  in  subsequent  sections. 


1.  Standard  UKF 

Following  the  unscented  transformation  (UT)  algorithms  from  [13],  consider  a 
random  variable  x  e  ,  where  L  is  the  number  of  states  being  estimated,  that  is 
propagated  through  an  arbitrary  nonlinear  function  y  =  /2(x).  If  x  has  an  assumed  mean 

of  X  and  covariance  ,  the  statistics  of  y  can  be  calculated  by  first  forming  a  matrix 
X  of  2T  + 1  sigma  vectors  according  to 


where  T  is  a  scaling  parameter  calculated  by 

X  =  a\L^K)-L  (3.18) 

The  parameter  a  determines  the  spread  of  the  sigma  points  around  the  mean  x  and  is  a 
tunable  parameter  that  is  generally  set  to  values  1x10  <  a  <1.  The  parameter  /r  is  a 
secondary  scaling  parameter  that  is  generally  set  to  values  of  k  =  3 -Lor  zero.  The  term 

in  Equation  (3.17)  represents  the  ithrow  or  column  of  the  matrix  square 

root  depending  on  the  function  used  to  calculate  it.  Using  MATLAB’s  chol  function  to 
calculate  the  matrix  square  root  leads  to  using  the  hh  row. 

Next,  the  sigma  vectors  from  Equation  (3.17)  are  individually  passed  through  the 
nonlinear  function  to  form 
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=  i  =  0,...,2L  (3.19) 

Lastly,  the  mean  and  eovarianee  for  y  are  approximated  using  a  weighted  sample  mean 


and  eovarianee  of  the  transformed  sigma  points  by 


i=0 

?=0 

The  sigma  weights  are  ealeulated  aeeording  to: 


(3.20) 

(3.21) 


(«»)  _ 


L  +  A 


L  +  A 


+  (1  ~  CC^  +  y0) 


= = 


1 


2(L+;i) 


i  =  0,...,2L 


(3.22) 


where  ^  is  determined  by  the  assumed  probability  distribution  of  x.  For  Gaussian 
distributions  =  2  is  the  optimal  value.  The  UT  ean  be  extended  to  the  problem  of  state 
estimation,  forming  the  unseented  Kalman  filter. 


For  a  state  veetor  x  e  where  n  is  the  number  of  states  being  estimated,  let  the 
nonlinear  system  given  by 


z,=h{x„w,) 

v, -N{0,Q) 

w, -N[Q,R) 

Let  an  augmented  state  estimation  veetor  of  dimension  L  be  defined  as 


(3.23) 


K-i=\jy-x  (3.24) 

where  the  augmented  state  veetor  is  extended  to  inelude  all  measurement  and 
proeess  noise  terms. 
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The  state  error  eovariance  matrix  is  also  extended  to  inelude  the  measurement  and 


proeess  noise  matriees; 


P"  = 


0 

0 


0  0 
Q  0 

0  R 


The  2L  +  \  sigma  veetors  are  ealeulated  following  Equation  (3.17)  : 


(3.25) 


= 


^k-\  ^k-l  -\/(f'  +  ^)Pk-\  ^k-\ 


(3.26) 


The  sigma  veetors  are  then  propagated  through  the  nonlinear  system  proeess 
defined  in  Equation  (3.23)  using  the  weights  from  Equation  (3.22)  to  ealeulate  the  a 
priori  transformed  mean  and  eovarianee  of  the  state  and  measurement  estimates 
according  to; 


(3.27) 

The  2E+1  estimates  are  used  to  create  a  weighted  mean  value  for  the  state  at  time 
k: 


i>=f.^i"'x;,  (3.28) 

;=0 

The  covariance  of  the  transformed  process  is  calculated  by  executing  a  weighted 
expected  value  operation  between  the  transformed  sigma  points  and  the  weighted  mean; 

Pk  =  Z  [xlk  -  Xk  \xlk-  Xk  J  (3 -29) 

/=0 

Similarly,  the  transformed  sigma  points  are  used  in  the  measurement  process  to 
calculate  the  weighted  mean  value  of  the  measurement  estimate  according  to; 


Y 


k 


(3.30) 
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The  covariance  of  the  measurement  process  is  estimated  by  executing  a  weighted 
expected  value  between  the  transformed  measurement  estimates  and  weighted  mean  of 
those  estimates: 


/=0 


(3.31) 


The  cross  covariance  between  the  state  process  and  measurement  process  is 
approximated  as  the  weighted  covariance: 


n..,  (3.32) 

i=0 

With  the  time  update  complete  and  covariance  determined,  the  state  corrections 
can  now  be  implemented  via  the  standard  Kalman  filter  measurement  update  equations. 
The  Kalman  gain,  however,  is  calculated  using  the  cross  covariance  and  measurement 
covariance  instead  of  solving  the  algebraic  Riccati  equation  as  in  the  EKF 
implementation: 


(3-33) 

The  state  and  state  covariance  is  lastly  corrected  in  the  same  manner  as  the  EKF: 


h  =^k+K,{z,-z,^) 
=P  -K  P 

^  k  ^  k  ^^k^  k  ^^k 


This  algorithm  is  initialized  by 


(3.34) 


P,=E 


x,=E[x,] 
x;=[xl  0  o]^ 

(Xo-Xo)(Xo-Xo)' 


— 

''0  “ 


Po  0  0 
0  0  0 
0  0  R 


(3.35) 


where  the  process  and  measurement  noise  are  set  to  their  mean  value  of  zero  and  the 
process  and  measurement  noise  matrices  augment  the  covariance  matrix  used  to  create 
the  sigma  point  vectors. 
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The  standard  UKF  algorithm  for  state  estimation  is  summarized  in  Figure  10.  In 
general  this  algorithm  is  computations  which  can  be  computationally  intensive  for 

systems  with  large  state  vectors. 


Calculation 

r=[(rf  (z^rj 


Initial  Values 

■’fo  =  '£'K] 
•Vo=[-vi'  0  o]'' 

0  0 
0  p  0 
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'  Sigma  Weights 

A  =  a'(Z  +  \)-L 
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n-r  =  - 
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i-O 
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> 
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iv  =  £  fi',"'  [za  -  -t;  ][z,'*  -  v;  ]‘ 
Y.=w(zfzn 


Figure  10.  Standard  UKF  recursion. 


2,  Square  Root  UKF 

For  the  standard  UKF  algorithm,  the  most  computationally  intensive  operation  is 
creating  the  sigma  vectors  at  each  time  step,  which  requires  calculating  the  matrix  square 
root  of  the  augmented  error  covariance  matrix  executed  by  the  MATLAB  chol  function. 
The  Cholesky  factorization  function  chol  uses  O  (zV  6)  computations  and  computes  the 

matrix  square  root  of  the  state  covariance  matrix  given  by  SS^  =  P .  The  square  root  UKF 
(SR-UKF)  propagates  S  directly  by  using  the  linear  algebra  techniques:  QR 
decomposition,  Cholesky  factorization  updating,  and  efficient  least  square.  QR 
decomposition  is  a  technique  used  to  calculate  a  matrix  square  root  and  requires  only 

computations,  where  N  is  the  number  of  sigma  vectors.  The  Cholesky  factor 
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update  corrects  the  output  of  the  QR  decomposition  and  requires  only 

computations.  In  general,  the  SR-UKF  computations  are  of  the  same  order  of  magnitude 
as  the  standard  UKF,  but  provide  better  numerical  properties  such  as  a  guaranteed 
positive-semidefinite  state  covariance  matrix  [14],  This  is  an  important  quality  as  the 
numerical  stability  of  the  UKF  depends  on  the  state  error  covariance  matrix  remaining 
positive-semidefinite. 

Using  the  same  system  described  in  Equation  (3.23)  and  the  augmented  state 
vector  in  Equation  (3.24),  the  augmented  Cholesky  factor  is  defined  as 


0  0 
0  4q  0 
0  0  Vr 


(3.36) 


The  sigma  point  vectors  are  created  with  the  same  methods  from  the  standard 
UKE  with  the  exception  that  the  matrix  square  root  of  the  covariance  is  replaced  by  the 
Cholesky  factor; 


xU  = 

4-1  4 

_^+y][L+x)s^  4-1  “V 

\L  +  A)s:_ 

II 

1  1 

to 

>5 

1 _ 1 

T 

(3.37) 


The  time  update  process  is  carried  out  in  the  same  manner  as  the  standard  UKE 
with  the  exception  that  the  state  covariance  estimate  calculation  step  is  replaced  by  the 
time  update  of  the  Cholesky  factor  using  the  QR  decomposition  of  the  weighted 
sigma  vectors  and  subsequent  Cholesky  factorization  update  or  downdate  depending  on 
the  value  of  the  weight  value.  The  QR  decomposition  is  represented  as  the 
MATEAB  operator  qr,  likewise  the  Cholesky  update  is  represented  by  the  MATEAB 
operator  cholupdate: 
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(3.38) 


Xl=f(xlvXtvU,) 


i=0 


S,=qr 


cholupdate  1 5*^^  ( xlk  “  ) .  sign  ( )| 

The  measurement  update  equations  are  the  same  as  the  UKF,  but  replace  the 
calculation  of  the  measurement  error  covariance  with  the  QR  decomposition  and 
Cholesky  update; 
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5. =Z’r'”’Tf, 


(=0 


=  V 


(3.39) 


S,  = 


cholupdate  1 5*5^ ,  ( 


The  error  cross  covariance  calculation  is  the  same  as  the  UKF  implementation. 
Efficient  least  squares  is  used  to  back-solve  for  the  Kalman  gain  through  the  use  of 
MATLAB’s  “/”  operator; 


(3.40) 


The  measurement  correction  step  is  executed  in  the  same  manner  as  the  UKF,  but 
the  correction  to  the  error  state  is  executed  with  a  Choleskly  downdate  according  to; 

=x,+K,{zi^-Zi^) 

U  =  K,S,^  (3.41) 

=  cholupdate ,U 

The  SR-UKF  recursion  is  summarized  in  Figure  1 1  and  initialized  by 
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5*0  =  chol  I E 


(Xo-Xo)(Xo-Xo) 

Xo  =^[xo] 

Xo  =  [xo  0  o]^ 


ca  _ 
‘^0 


^0  0  0 
0  4q  0 
0  0  4r 


(3.42) 
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Figure  11.  SR-UKF  recursion. 


3,  Spherical  Simplex 

As  both  the  standard  UKF  and  SR-UKF  are  of  the  same  computational 
complexity  of  O  j ,  one  way  to  reduce  the  number  of  computations  is  to  use  less  sigma 

point  vectors  to  estimate  the  state  mean  and  covariance.  The  spherical  simplex  method  of 
selecting  sigma  points,  developed  by  [15],  requires  only  Z+2  sigma  vectors  vice  the  2Z+1 
required  in  both  the  UKF  and  SR-UKF  by  rearranging  the  sigma  points  on  a  hyper¬ 
sphere. 
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First,  the  zeroth  weight  is  ehosen  sueh  that  0<Wq<1.  This  weight  only  affeets 

the  fourth  and  higher  order  moments  of  the  sigma  point  set.  The  remaining  weights  are 
ealeulated  by 

^  ih3l  /  =  !,.. .,1  +  1  (3.43) 

'  L  +  \ 

The  weights  ean  be  sealed  aeeording  to 


w,  =  < 


a 


El 


for  i  =  0 
for  i  0 


(3.44) 


whieh  helps  to  mitigate  the  effeet  of  higher  order  terms. 

Lastly,  the  zeroth  weight  of  the  sealed  weight  set  ean  be  modified  to  ineorporate 
prior  knowledge  of  the  probability  distribution  by 


.  (3.45) 

With  the  sealed  spherieal  weights  developed,  the  simplex  matrix  for  seleeting  the 
sigma  points  is  ereated  using  the  algorithm 


z;=[o],z; 


-1 

,z‘  = 

1 

_V2wi  J 

9  2 

_V2wi  _ 

■^0 

0 

z/-' 

-1 


Vi  (i+ 1)^1 


o.-i 
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Vi  (i+ 1)^1 


for  i  =  0 


for /  =  !,..., 7 


for  i  =  7  +  1 


(3.46) 
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The  spherical  simplex  sigma  point  vectors  for  the  spherical  simplex  UKF 
(SSUKF)  are  created  by 

(3.47) 

The  square  root  spherical  simplex  UKF  (SR-SSUKF)  sigma  point  vectors  are 
created  according  to 


=  r"  +7 

/Ck-l  -^k-l  ^  ^i‘^k-1  ■ 


(3.48) 


The  algorithms  for  the  SSUKF  and  SR-SSUKF  are  identical  to  the  standard  UKF 
and  SR-UKF,  respectively,  utilizing  the  spherical  weights  from  Equation  (3.45)  and 
sigma  point  selection  from  Equations  (3.47)  or  (3.48). 
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IV.  SIMULATION  TOOLS  AND  ASSUMPTIONS 


A,  INTRODUCTION 

In  order  to  properly  investigate  the  qualities  of  eaeh  navigation  estimator,  a 
eontrolled  simulation  environment  was  used  to  gather  and  analyze  data.  Utilizing  a 
simulation  environment  required  eertain  model  and  equation  simplifieations,  but 
parameter  values  were  ehosen  to  mimie  the  true  SEAFOX  II  data  as  elose  as  possible 
with  respeet  to  sensor  speeifieations. 

B,  SIMULATION  ENVIRONMENT  AND  ASSUMPTIONS 

1,  Simulation  Environment 

Laeking  a  truth  model  for  data  reeovered  from  the  SEAFOX  II  sea  trials,  a  robust 
simulation  environment  was  used  to  evaluate  and  test  eaeh  estimator.  Condor:  The 
Competition  Soaring  Simulator  is  a  eommereially  available  flight  simulator  that 
ineorporates  the  use  of  a  six  degrees  of  freedom  (6DOF)  flight  model  and  high  fidelity 
physies  engine  that  operates  up  to  500  eyeles  per  seeond  [17].  An  applieation  program 
interfaee  (API)  was  reeently  developed  that  exports  real-time  flight  telemetry  data  from 
Condor  to  MATEAB/Simulink  and  imports  eontrol  surfaee  eommands  from  Simulink  to 
Condor. 

The  following  states  of  interest  are  available  through  the  API  at  a  frequeney  of 
100  Hz: 

X  =  [(!), X,h,x^,y^,z^,v^,v^,v^,(p,6,\l/,p,q,r,Vj.\  (4.1) 

•  are  latitude,  longitude,  and  altitude  in  degrees  and  meters, 
respeetively. 

•  i^n’Se’^d)  are  the  FTP  eoordinates  in  meters. 

•  (u XeXd)  are  the  FTP  veloeity  eomponents  in  m/s. 

•  {(p,9,  y/)  are  the  roll,  piteh,  and  yaw  Euler  angles  measured  in  radians. 
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•  iyP,q,  r)  are  the  roll,  piteh,  and  yaw  rates  measured  in  radians  per  seeond. 

•  Vj.  is  the  true  airspeed  in  m/s. 

Due  to  software  limitations  in  the  API,  the  LTP  frame  aeeeleration  states  were  not 
available  during  the  development  of  this  thesis. 


2.  Simulation  Assumptions  and  Sensor  Models 

It  is  assumed  that  the  Condor  simulation  environment  uses  a  non-rotating  flat 
earth  model,  therefore  sidereal  veetor  in  the  LTP  frame  u)'.  =  0  .  Condor  does  not  provide 
organie  aeeelerometer  measurements  so  these  measurements  were  approximated 
following  the  derivation  of  speeifie  foree,  F* ,  from  [18].  LTP  aeeeleration  was 
approximated  using  order  filter  shown  in  Figure  12,  and  the  rotation  matrix  from  LTP 
to  body  frame  was  formed  with  the  available  Euler  angles  using  Equation  (5.13): 

F‘’=R^  —V‘  +colxr -R^g‘ .  (4.2) 

ydt  ) 

The  Simulink  representation  of  the  filter  used  to  estimate  ETP  aeeeleration  is 
shown  in  Eigure  12  and  has  the  state  spaee  representation 

ij  0  1  0  Xj  0 

^2=0  0  1  X2+0m  (4.3) 

L-^1  -^2  -^3jL^3j  lK_ 

where. 


K  = 

k,=2^col 

K  = 

K  = 

and  a>^  is  the  natural  frequeney  and  4”  is  a  damping  eoeffieient. 


(4.4) 


The  filter  bandwidth  values  were  empirieally  determined  to  be  co^  =10  rad/s  and 


^  =  1.5. 
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Figure  12.  3'^'*  order  filter  for  GPS  aoeeleration  estimation. 


In  an  effort  to  make  the  simulated  gyro  and  aeeelerometer  measurements  more 
realistie,  normally  distributed  band-limited  white  noise  and  eonstant  biases  were  added  to 
eaeh  channel.  The  resulting  gyro  simulation  sensor  model  used  throughout  the  simulation 
trials  was 


L=0  .  (4.6) 

The  LTP  position,  speed,  and  heading  from  the  output  state  vector  were  also 
corrupted  with  additive  band-limited  white  noise  resulting  in  the  simulation  models 

P‘=P‘  +  v^,  v^-N(0,al)  (4.7) 

V‘  =V‘+v^,  v^~n(0,  al )  (4.8) 
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¥=¥  +  \,  v^~iV(0,f7^)  (4.9) 

The  power  speetral  density  (PSD),  defined  as  the  varianee  of  the  noise,  and  bias 
values  used  for  eaeh  simulated  sensor  throughout  all  the  simulations  are  in  Table  3.  All 
values  were  ehosen  to  elosely  mimie  their  eounterpart  onboard  the  SEAFOX II. 


Sensor  Noise  and  Bias  Values  1 

For  Condor  Simulation 

Sensor 

PSD 

Bias 

Gyro 

=  (O.Ol  rad/s)^ 

Z,  =[2.4x10^"  -1x10^  2x10^7 

L  J 

Aecelerometer 

=(0-l  m/s")' 

h,,  =[l.5xl0^"  -1.5x10^"  lxl0^"7 

GPS 

cjI={1>2  m)" 

0 

GPS  Speed 

(jI  =  (^.44  m/s") 

0 

GPS  Heading 

al  =(0.001  rad)" 

0 

Table  3.  Values  for  simulated  sensor  PSD  and  bias. 
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V.  REFERENCE  FRAMES 


A,  OVERVIEW 

The  design  of  a  navigation  system  requires  the  transformation  of  sensor  data 
between  various  frames  to  a  common  frame  of  reference.  The  accelerometer,  gyroscope, 
and  GPS  measurement  data  must  be  transformed  from  their  respective  measurement 
frames  of  reference  to  a  common  frame  prior  to  integration.  Four  frames  of  reference  and 
the  methods  to  transfer  between  systems  are  discussed  in  this  chapter  and  used  in  the 
subsequent  navigation  system  design:  Earth  centered  inertial  (ECI),  Earth  centered  Earth 
fixed  (ECEE),  geographic  (or  navigation),  and  body  frame. 

B,  REFERENCE  FRAME  DEFINITIONS 

This  section  covers  a  brief  overview  of  the  four  reference  frames  used  throughout 
this  thesis.  All  reference  frame  equations,  transformations,  and  descriptions  covered  in 
this  chapter  are  derived  from  chapter  2  of  [8],  which  provides  a  thorough  analysis  of  the 
topic. 


1,  Earth  Centered  Inertial 

In  the  inertial  frame  of  reference,  Newton’s  second  law  of  motion  applies.  The 
point  of  origin  for  an  inertial  frame  of  reference  is  arbitrary,  but  the  three  coordinate  axes 
must  be  orthogonal  to  one  another.  Inertial  sensors,  such  as  an  accelerometer  and 
gyroscope,  produce  measurements  relative  to  an  inertial  reference  frame,  resolved  along 
the  sensor’s  measurement  axis. 

The  ECI  reference  frame  is  a  non-rotating  frame  with  its  origin  located  at  the 
Earth’s  center  of  mass.  The  z-axis  is  defined  as  parallel  to  the  Earth’s  rotation  axis.  The 
x-axis  is  perpendicular  to  the  z-axis,  intercepting  the  sphere  of  the  earth  at  0°  latitude. 
The  y-axis  is  orthogonal  to  both  the  x-axis  and  z-axis  forming  a  right-handed  coordinate 
system.  See  Eigure  13  for  further  details. 


33 


2. 


Earth  Centered  Earth  Fixed 


The  Earth  centered  Earth  fixed  (ECEE)  reference  frame  is  defined  similarly  to  the 
ECI  frame  in  that  the  origin  is  collocated  at  the  center  of  Earth’s  mass  and  the  z-axis  is 
parallel  to  the  Earth’s  rotation  axis.  The  ECEE  x-axis  is  fixed  at  the  intersection  of  the 
Earth’s  sphere  at  0°  latitude  and  O'’  longitude.  The  y-axis  is  perpendicular  to  both  the  z- 
axis  and  x-axis  to  complete  a  right-handed  coordinate  system.  See  Eigure  13  for  further 
detail.  The  ECEE  frame  rotates  relative  to  the  ECI  frame  with  frequency: 


^1-1-365.25  cycle  V In  rad/cycle^ 


(365.25x24)  hr 


3600  sec/hr 


=  7.292115x10 


-5  rad 


sec 


(5.1) 


3,  Geographic  (Navigation)  Frame 

The  navigation  frame  is  a  local  reference  frame  that  moves  with  the  vehicle, 
relative  to  the  Earth’s  geoid.  It  is  defined  by  the  projection  of  the  vehicle’s  origin  onto  the 
Earth’s  reference  ellipsoid  as  shown  in  Eigure  13.  The  z-axis  points  from  the  platform 
origin  towards  the  origin  of  the  reference  ellipsoid.  The  x-axis  points  towards  true  north 
along  the  plane  orthogonal  to  the  z-axis.  The  y-axis  points  east,  perpendicular  to  both  the 
x-axis  and  z-axis,  completing  the  right-handed  coordinate  system. 
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Figure  13.  Navigation,  ECEF,  and  ECI  coordinate  reference  frames,  after  [19]. 


4,  Body  Frame 

The  body  frame  is  rigidly  attached  to  the  vehicle.  For  this  thesis,  the  body  frame 
origin,  vehicle  center  of  gravity,  and  sensor  origins  are  assumed  to  coincide  which 
simplifies  the  transformations  between  sensor  measurements  and  the  frame  the 
measurements  are  used  in.  The  body  frame  x-axis,  shown  as  the  u-axis  in  Figure  14, 
points  towards  the  front  of  the  vehicle.  The  normal  direction  of  vehicle  motion  is 
assumed  to  be  in  the  x-direction.  The  z-axis  is  perpendicular  to  the  x-axis  and  points  at 
the  bottom  of  the  vehicle.  The  body  y-axis  is  perpendicular  to  both  the  z-axis  and  y-axis 
to  complete  the  right-hand  coordinate  system. 

The  rotation  rate  vector  of  the  body  frame  relative  to  the  inertial  frame,  resolved 
along  the  body  axes  is  defined  as  col-  =  [p,q,rY ,  where  p  is  the  angular  roll  rate  about 
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the  x-axis,  q  is  the  angular  pitch  rate  about  the  y-axis,  and  r  is  the  angular  yaw  rate  about 
the  z-axis.  Each  angular  rate  is  positive  in  the  right-hand  sense  as  shown  in  Figure  14. 


Figure  14.  Body  reference  frame  representation,  from  [8]. 

5,  Local  Tangent  Plane 

The  local  tangent  plane  (FTP)  is  a  convenient  frame  of  reference  for  navigation  is 
local  areas.  The  FTP  frame  uses  a  right  hand  north,  east,  down  rectangular  coordinate 
system  where  the  origin  is  located  in  the  vicinity  of  the  vehicles  operating  area.  For  a 
stationary  system  located  at  the  FTP  fame  origin,  the  FTP  frame  and  navigation  frame 
coincide. 

C.  EARTH  REFERENCE  ELLIPSOID 

An  accurate  model  of  the  Earth  is  vital  to  the  accuracy  of  a  navigation  system. 
Figure  15  depicts  a  representation  of  the  Earth’s  reference  ellipsoid. 
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The  ellipsoid  is  defined  by  is  semi-major  axis  (a)  and  its  semi-minor  axis  (b). 
The  axis  values  are  defined  by  the  WGS84  geodetie  system  and  shown  in  Table  4. 


WGS84  Parameters 

Parameter 

Symbol 

Value 

Units 

Semi-major  Axis 

a 

6378137 

Meters 

Semi-minor  Axis 

b 

6356752.314 

Meters 

Flatness 

a 

0.00335281 

Eeeentrieity 

e-V/(2-/) 

0.08181919 

Table  4.  WGS84  parameter  eonstants,  from  [8]. 


For  a  point  P  on  the  Earth’s  surfaee  there  is  a  north-south  meridian  plane  of 
eonstant  longitude  that  intereepts  the  Earth’s  ellipsoid.  If  arbitrary  point  P  has  a  latitude 
of  ^  and  longitude  X  then  the  meridian  radius  ean  be  deseribed  as: 


(l-e^  sin^  {(f)Y 


(5.2) 
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For  the  same  point  P,  the  prime  vertiele,  an  east-west  vertieal  plane  that  intereepts 
the  referenee  ellipsoid  as  shown  in  Figure  15  that  is  normal  to  the  meridian  plane,  has  a 
radius  of; 


a 


(5.3) 


jl-e^  sin^  {^(f) 

These  radii  are  used  for  eoordinate  transformation  between  geodetie  eoordinates 
and  the  ECEF  eoordinates  ,  where  h  is  the  altitude  of  P  above  the 

referenee  geoid. 


D.  ECEF  TO  NAVIGATION  FRAME  TRANSFORMATION 

The  navigation  frame  is  a  unique  frame  in  that  the  origin  of  the  navigation  frame 
moves  with  the  vehiele  sueh  that  the  vehiele’s  position  expressed  in  the  navigation  frame 
is  A"  =  [0, 0,  -hf .  Eatitude  (j)  and  longitude  A  define  the  origin  of  the  navigation  frame 
on  the  reference  ellipsoid.  To  obtain  the  earth  relative  velocity  vector  of  the  vehicle  in  the 

navigation  frame,  the  ECEE  velocity  vector  F/=— A^must  be  transformed  to  the 

dt 

navigation  frame; 

f;  =  Ry:  (5.4) 

The  components  of  the  velocity  vector  V"  =  [v„,v^,v^]^  represent  the  instantaneous 
north,  east,  and  down  velocity  components  along  the  navigation  frame  axes.  Since 
latitude,  longitude,  and  altitude  are  the  common  values  output  by  GPS  receivers,  the 
relationship  between  ECEE  and  geodetic  position  must  be  established  and  used  to  relate 
V"  to  the  rate  of  change  of  latitude,  longitude,  and  altitude 

Using  the  radii  from  Equations  (5.2)  and  (5.3),  and  the  eccentricity  constant  from 
Table  3,  the  relationship  between  ECEE  position  and  geodetic  position  is  defined  as; 

A  =(^7v  +  ^)cos(^zi)cos(/l) 

A  =(^v  +  ^)cos(^/5)sin(/l)  (5.5) 

A=[^v(l-e')  +  /?]sin(^/5) 
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The  velocity  vector  F/  can  be  expressed  as; 


F,  =—(!)  +— A +  —  h 


a^ 


dA 


dh 


where  the  partial  derivatives  can  be  expressed  as 


dX^ 


[Rm  +h) 


[Rf^+h) 


-sin(^zi)cos(/l) 
-sin(^zJ)sin(/l)  , 
cos(^zJ) 

-cos(^zJ)  sin(/l) 
cos(^zJ)  cos(/l) 

0 


dX^ 


cos(^zi)  COS(/l) 

cos(^zJ)sin(/l) 

sin(^zi) 


Equation  (5.7)  can  then  be  rearranged  to; 


1 — 

o 

o 

+ 

1 _ 

V"  =R" 

e  n 

0  +/2)cos(^z^)  0 

i 

0  0-1 

h 

Finally,  Equation  (5.8)  can  be  reduced  to  show  the  relation  in  Equation  4; 


(5.6) 


(5.7) 


(5.8) 


1 

? 

+ 

_ 1 

V"  = 

e 

= 

cos(^z^)(7?^  +h)A 

-h 

In  differential  equation  form,  Equation  (5.9)  becomes; 


<!> 


{Ru+h) 


h 


{Rf^+h)cos{(l)) 


(5.9) 


(5.10) 


E.  NAVIGATION  TO  BODY  FRAME 

The  body  frame  is  related  to  the  navigation  frame  through  the  Euler  angles 
{6,(p,y/)  pitch,  roll,  and  yaw,  respectively.  A  direction  cosine  matrix  (DCM)  which  is 
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used  to  transform  vectors  from  the  body  to  navigation  frame  can  be  formed  by  three 
plane  rotations  as  shown  in  Figure  16. 


Figure  16.  Plane  rotations  of  yaw,  pitch,  and  roll  from  navigation  to  body. 


The  three  plane  rotations  from  body  to  navigation  frame  are  calculated  by: 


cos(^i^)  sin(^)  0 
-sin(^)  cos(^//')  0 

’^y,d 

cos{d)  0  -sin(6') 

0  1  0 

L  0 

0  1 

sin(6’)  0  cos(6’) 

'1  0 

0  1 

R 


x,(p 


0  cos(^3)  sin(^) 

0  -sin(^)  cos(^3) 


(5.11) 


To  create  the  DCM,  ,  the  plane  rotations  from  Equation  (5.11)  are  multiplied 


in  a  32—1  sequence 


resulting  in  the  rotation  matrix: 

cy/c6  SI//C0  -sd 

cy/s6s(p  -  sxj/cq)  sy/s6s(p  +  cy/c(p  cOs(p 
of/sOcq)  +  sxj/sq)  sy/s6c(p  -  cy/s(p  cOccp 

where  s  =  sin(-),  and  c  =  cos(-) . 


(5.12) 


(5.13) 
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This  rotation  matrix  has  properties  such  that  the  rotation  matrix  that  transforms 
vectors  from  the  body  to  navigation  frame  is  the  transpose  of  the  defined  DCM; 

«;=(«!)'  (5.14) 

The  Euler  angles  {^(p,6,y/)  can  be  recovered  from  the  DCM  in  Equation  (5.13) 
via: 


(p  =  tan 


y/  =  tan 


^!(2,3) 

V^'(3,3), 


^  =  -sin-'(i?„^(l,3)), 


V 


where  R^(row,colum)  marks  the  appropriate  matrix  indices. 


(5.15) 


Eor  local  navigation  in  the  vicinity  of  the  local  tangent  plane  origin,  the  rotation 
matrices  and  Euler  angles  can  be  assumed  to  be  equal  resulting  in 


K=K 


F.  ECEF  TO  LOCAL  TANGENT  PLANE 


(5.16) 


A  vehicle’s  arbitrary  position  in  the  ECEF  frame  is  defined  as 
^vehicle  -  i^e  Te  ^ ef  ’  ^^*3  thc  origiii  of  thc  ETP  frame  resolved  in  the  ECEF  frame  is 

defined  as  X^j-p  =  [x^  f  .  The  vector  that  connects  the  ETP  origin  to  an  arbitrary 

ECEF  position  is 

(5-lV) 

Two  plane  rotations  are  required  to  transform  a  vector  defined  in  ECEF  to  ETP. 
First  the  vector  is  rotated  about  the  ECEF  z-axis  by  the  angle  A  (longitude)  then 
vertically  by  angle  (j)  (latitude)  to  form  the  rotation  matrix  from  ECEF  to  ETP  as 


cA 

sA 

o' 

-S(j) 

0 

C(l) 

-s(j)cA 

-s(j)sA 

C(l) 

K  = 

-sA 

cA 

0 

0 

1 

0 

= 

-sA 

cA 

0 

0 

0 

1 

C(j) 

0 

S(j) 

-c(l)cA 

-c(j)sA 

-S(j) 
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To  resolve  a  vehicle’s  position  in  LTP,  Equation  (5.17)  is  rotated  by  the  rotation 
matrix  in  Equation  (5.18)  resulting  in 


(5.19) 
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VI.  NAVIGATION  STATE  ESTIMATOR  DESIGN 


A,  OVERVIEW 

A  complimentary  filter  fuses  the  information  from  multiple  sensors  operating  at 
various  frequencies  to  produce  a  combined  estimate  of  a  desired  state  or  parameter.  In 
this  implementation,  a  high  rate  sensor  is  used  as  the  input  to  a  system  process  and  the 
output  is  correeted  by  a  seeond  sensor  that  operates  at  a  mueh  lower  data  output  rate.  For 
attitude  estimation,  the  rate  gyro  measurements  are  integrated  to  estimate  the  vehicle’s 
orientation.  Depending  on  the  quality  of  the  gyro,  these  estimates  ean  significantly  drift. 
Likewise,  the  aceelerometer  measurements  are  integrated  once  for  velocity  estimates  and 
a  second  time  for  position  estimates  which  will  lead  to  large  dead  reckoning  errors 
without  correction  from  an  external  souree.  Further  discussion  on  complimentary  filters 
can  be  found  in  chapter  7  of  [8]. 

B,  PROCESS  MODEL 

In  a  standard  combined  position,  velocity,  and  attitude  process  model  as  described 
in  chapter  1 1  of  [8],  the  number  of  estimated  states  for  even  a  simple  navigation  system 
can  number  around  15  when  considering  three  dimensional  position,  velocity,  and 
attitude,  and  gyro  and  aceelerometer  bias  errors.  The  unscented  Kalman  filter  proeess 
augments  the  already  large  state  vector  with  process  and  measurement  noise  terms, 
growing  the  state  veetor  to  30  states  or  more.  When  considering  that  the  UKF  must  iterate 
2L+\  or  L+2  times  per  time  step  depending  on  the  sigma  point  selection  method,  where  L 
is  the  number  of  states,  it  is  clear  that  reducing  the  number  of  states  is  paramount  to 
developing  a  practical  real-time  navigation  estimator  that  features  the  UKF  framework. 
To  this  end  the  process  model  was  split  into  an  attitude,  heading,  and  reference  system 
(AHRS)  and  an  inertial  navigation  system  (INS),  reducing  the  number  of  states  per 
estimator. 

The  cascaded  estimator  is  broken  into  three  major  categories;  sensor 
measurements,  attitude  determination,  and  position  and  velocity  tracking.  The  eascaded 
process  model  used  in  each  estimator  is  depicted  in  Figure  17.  This  cascaded  design 
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closely  follows  the  work  presented  in  [20],  [21],  and  [22],  The  AHRS  module  reeeives 
inputs  from  the  GPS,  IMU,  and  previous  time  step  estimates  from  the  INS  module  and 
updates  the  vehiele’s  attitude.  The  updated  attitude  estimates  are  used  to  form  a  DCM 
from  the  body  frame  to  the  LTP  referenee  frame,  whieh  is  used  by  the  INS  module  to 
update  position  and  velocity  estimates. 


The  state  veetor  for  the  AHRS  module  estimators  is: 


X 


AHRS 


^0  ^2  ^3  b 


gs,r 


(6.1) 


•  (^0  ^1  ^2  ^3)  the  quaternion  state  estimates  representing  vessel 

orientation. 


•  (4^,;^  4^.?  bgs,r )  are  the  gyro  bias  state  estimates. 

The  state  veetor  for  the  INS  module  estimator  is: 


Y  — 
^  INS 


X  y  z.  V  V  V,  b  b  b 

n  ye  a  n  e  a  as,x  as^y  as,z 


(6.2) 
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•  (^„  ye  the  north,  east,  and  down  LTP  position  estimates. 

•  (K  K  K)  the  north,  east,  and  down  LTP  veloeity  estimates. 

•  [Ks X  K.S y  Ksz)  the  body  x,  y,  and  z  channel  accelerometer  bias 
estimates. 

1.  AHRS 

The  output  of  the  IMU  gyro  is  an  angular  rate  measured  in  the  body  frame  with 
respect  to  the  inertial  frame,  represented  in  the  body  frame  •  The  angular  velocity  of 
the  body  frame,  with  respect  to  the  navigation  frame,  represented  in  the  navigation  frame 
contains  components  of  the  sidereal  rate  ((u”  j  and  transport  rate  represented 
by  the  angular  velocity  of  the  navigation  frame,  with  respect  to  the  inertial  frame, 
represented  in  the  navigation  frame  • 


0)1  =o)t-Ro}’'. 

bn  bi  n  ni 


CO^:  =  +  co..^  = 


ni  ei 


co^,cosi(/>) 

Xcos{</)) 

(i  +  ry^.)cos(</i) 

0 

+ 

-(p 

= 

-ij) 

sin(^)J 

-Xsm{(/)) 

-(i  +  ry^.)sin(^/i) 

(6.3) 

(6.4) 


where  <y is  the  sidereal  rate,  defined  as  the  angular  velocity  of  the  ECEF  frame,  with 


respect  to  the  inertial  frame,  represented  in  the  navigation  frame  at  the  latitude  of  the 
vehicle  navigation  frame  origin,  and  col^  \s,  the  transport  rate  of  the  navigation  frame 
origin  in  the  ECEE  frame  resolved  in  navigation  frame  coordinates  [8]. 

Equation  (6.4)  can  be  simplified  by  using  a  local  tangent  plane  (ETP)  assumption, 
denoted  with  the  superscript  t,  which  fixes  the  navigation  frame  to  a  constant  location 
making  =  0  resulting  in 


(Uj 

II 

0 

(O^ 

sin(^o) 
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The  standard  representations  of  three-dimensional  vehicle  attitude  are  rotation 
matrices  (or  DCMs),  Euler  angles,  and  unit  quaternions.  Rotation  matrix  update  equations 
are  linear  and  lack  singularities,  but  require  nine  differential  equations.  The  Euler  angle 
representation  requires  only  three  differential  equations,  but  uses  non-linear  equations 
and  contains  a  singularity  when  pitch  passes  90  degrees.  Elnit  quaternions  use  four 
differential  equations,  but  the  equations  are  linear  and  lack  any  singularities.  Elnit 
quaternions  are  used  in  the  subsequent  AHRS  design  due  to  the  simplicity  of  their 
differential  equations  and  ability  to  easily  construct  Euler  angles  and  rotation  matrices 
[23]. 

Eollowing  the  quaternion  derivations  outlined  in  [23],  the  unit  quaternion 
kinematic  equation  for  attitude  representation  using  the  angular  rate  as  described  by 
Equation  (6.5)  is 

0  -(Uj  -rUj 

(Uj  0  co^  -co^ 

0)^  -co^  0  rUj 

co^  co^  -rUj  0 

where  is  a  skew  symmetric  operator. 

A  DCM  from  navigation  or  ETP  to  body  frame  can  be  formulated  by  using  the 
relation 

r"  -  qxr'’ xq*  -  R^r'’  (6.7) 

where  r  =  ^r^  is  a  three-dimensional  vector,  q  =  \qQ  q^  ^2  q^^  is  the  unit 

quaternion,  and  q*  =  ^q^  -q^  -q^  -q^^  is  the  unit  quaternion  complex  conjugate. 


0 

1 

2 
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The  resulting  DCM  is 


r:  = 


ql+qf 


2 

-^2 


2{q^q^+q^q^) 

2{q,q^-q^q2) 


2  (^1^2 -^0^3) 

ql-ql+ql-ql 

2(g'2^3+9o^l) 

\T 


2(91^3+90^2) 
2  (  9293  -  9o9i) 
9o-9f-92+93 


The  Euler  angles  are  related  to  quaternions  through  the  DCM  as 


CJ-tan‘  -tan'  2(^293  +9o9i) 

e  =  sin"'  (-i?*  (1,3))  =  sin"'  (-2(9193-9092)) 

2(9i92  +  9o93  ) 


y/  =  tan" 


=  tan"' 

U‘(u)J 

V 

2  ,  2 

9o  +  9i 


92 


2  , 
■93 ; 


(6.8) 


(6.9) 


In  order  to  maintain  aeeuraey,  the  unit  quaternion  must  be  normalized  after  eaeh 
update  by 


9  =  ^  (6.10) 

V9  9 

to  ensure  that  ||9||  =  1  at  every  time  step. 

2,  INS  Position  and  Velocity 

In  order  to  traek  the  vessel’s  eurrent  position  and  veloeity,  an  accelerometer  is 
integrated  through  a  series  of  equations  that  transform  the  measurements  to  the  required 
frame  of  reference.  Position  and  velocity  in  the  navigation  and  LTP  frames  are  derived 
here,  as  both  frames  were  considered  for  implementation  in  the  estimator  design.  The 
following  equations  for  position  and  velocity  are  based  on  the  derivations  described  in 
chapter  11  of  [8]. 

As  previously  derived  in  Equation  (5.10),  the  differential  equation  used  to 
estimate  the  vehicle’s  position  in  the  navigation  reference  frame  is; 
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V, 


h 


[Rm  +h) 


+ /z)  cos(</5) 


(6.11) 


The  LTP  frame  origin  is  locked  into  a  constant  location,  therefore  position  can  be 
determined  by  directly  integrating  velocity: 


1 

5 

1 

t 

\ 

rt 

5 

_ 1 

II 

•Ck 

ye 

= 

1 

N- 

SL 

1 _ 

1 

Si, 

1 _ 

(6.12) 


To  determine  a  vehicle  velocity  in  the  navigation  or  LTP  frame  it  must  first  be 
resolved  in  the  ECEF  reference  frame: 


=RIF’’ +  g^ -2[co:,xV^)  (6.13) 

where  col-  1^^^  sidereal  rate  of  the  ECEE  frame  with  respect  to  the  inertial  frame, 

represented  in  the  ECEE  frame,  and  F*  is  the  specific  force  as  measured  by  an 
accelerometer.  Specific  force  (or  g-force)  is  a  non-gravigational  force  per  unit  mass  with 
units  of  acceleration  (m/s  )  that  contains  linear,  centripetal,  and  gravitational  components 
of  acceleration. 


Since  the  navigation  frame  origin  moves  with  the  vehicle,  the  rate  of  change  of 
the  navigation  frame  with  respect  to  the  ECEE  frame  represents  the  vehicle  velocity  and 
is  described  by  the  differential  equation 

K=K{{<a>r-)  +  v-).  (6.14) 

Substituting  Equation  (6.13)  for  K  results  in 

1^;  =  +  g"  -  (<  +  2< )  X  F"  (6.15) 

where  is  the  transport  rate  of  the  navigation  frame  with  respect  to  the  ECEE  frame. 
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In  the  LTP  frame,  the  rate  of  ehange  of  the  tangent  plane  veloeity  with  respeet  to 
the  ECEF  frame  ean  be  deseribed  by 

v:=R((^a:,xV)  +  V-)  (6.16) 

where  o)®  =  0  sinee  the  EXP  origin  is  in  a  fixed  loeation. 

Substituting  Equation  (6.13)  for  E'^reduees  Equation  (6.16)  to 

V:=RlF''  +  g'-2[co:,^V‘).  (6.17) 

The  ETP  reference  frame  was  selected  as  the  desired  design  reference  frame 
onboard  the  SEAEOX  II  as  the  boat  normally  operates  in  local  waters  and  does  not  travel 
significant  distances  in  the  course  of  a  single  mission. 


3.  SEAFOX II IMU  Model 

The  accelerometer  and  gyroscope  in  the  IMU  are  not  ideal  sensors;  they  contain 
multiple  sources  of  error  and  noise.  Identifying  the  characteristics  of  the  noise  and  error 
sources  is  a  critical  component  of  an  accurate  and  robust  navigation  system.  Table  2  in 
Chapter  II  lists  the  manufacturer’s  analysis  of  the  IMU  noise  and  error  statistics.  By 
sampling  each  IMU  channel  over  a  short  time  interval  while  the  vehicle  is  stationary,  the 
lower  bound  of  the  noise  statistics  can  be  verified.  Figures  18  and  19  are  representative 
measurements  from  each  IMU  channel  over  a  10  second  sampling  period,  sampled  at  100 
Hz  data  output  rate  while  the  vehicle  is  stationary. 
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Accelerometer  Noise  Characteristics  Analysis 


01  23456789  10 
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Figure  18.  Measurements  from  the  IMU  accelerometer  channels. 
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The  standard  deviation  bounds  shown  in  Figures  18  and  19  are  the  standard 
deviations  of  the  data  set  shown,  not  of  the  manufacturer’s  specifications.  Using 
MATLAB’s  standard  deviation  command  std  to  analyze  the  data,  the  standard  deviation 
for  each  accelerometer  channel  while  stationary  is  =  [0.05  0.06  0.06]  m/s^  ,  which 

corresponds  to  approximately  5  milli-g’s,  a  common  unit  for  specific  force 
measurements.  These  standard  deviation  values  are  well  within  the  manufacture’s  100 
milli-g  noise  maximum.  All  of  the  sampled  data  points  fell  within  three  standard 
deviations  of  the  mean. 


51 


CD 


o 


2 

1 

0 

-1 

-2 


01  23456789  10 


Time  [s] 


X  10'^ 


X  10'^ 


Figure  19.  Measurements  from  the  IMU  gyro  roll  rate,  piteh  rate, 

and  yaw  rate  ehannels. 
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Gyro  measurements  were  acquired  at  the  same  time  as  the  accelerometer 
measurements.  The  gyro  measurements  are  much  better  distributed  around  the  mean  than 
the  accelerometer  measurements.  Again,  using  MATLAB  to  analyze  the  measurements 
the  standard  deviation  in  the  three  gyro  channels  are  <j^  =[5x10^  3.5x10^  4x10^]  rad/s, 

all  of  which  fall  well  below  the  manufacturer’s  maximum  noise  output.  Since  the  vehicle 
was  stationary  during  the  sampling,  the  mean  of  each  gyro  channel  serves  as  a  good 
estimate  for  initializing  the  gyro  bias  values.  The  initial  bias  of  the  gyro  is  not  always  the 
same  value  when  the  gyro  is  initialized,  so  the  bias  must  be  measured  at  each  restart  of 
the  IMU  in  order  to  initialize  the  estimator  with  accurate  values. 

In  an  effort  to  reduce  the  sensor  model  complexity,  all  noise  sources  are  assumed 
to  be  zero  mean  Gaussian  white  noise.  The  error  sources  in  each  channel  are  assumed  to 
be  additive  which  means  that  they  can  be  lumped  into  a  single  term.  To  account  for  noise 
sources  that  may  vary  with  time,  the  derivative  of  the  noise  terms  is  assumed  to  be  a 
Gaussian  white  noise  term  which  will  introduce  a  random  walk  to  the  bias  estimates; 


pb 


-g'+K+^u 


=  V, 


(6.18) 


Hi  -  Hi + 


K  =  \ 


(6.19) 


The  noise  terms  in  Equation  (6.18)  and  (6.19),  ~  and  ~  A(o,cr^) , 

respectively,  are  assumed  to  be  additive  zero  mean  Gaussian  white  noise  components  of 
the  accelerometer  and  gyro  measurements.  The  random  walk  is  described  by  the  zero 

mean  Gaussian  white  noise  processes  ~  )  and  j,  where  the 

cTj  and  cTj  standard  deviations  are  small  tunable  values. 


C.  MEASUREMENT  MODELS 

1.  AHRS  Measurement  Model  One 

Accelerometers  are  commonly  used  as  a  means  to  correct  drifting  gyro 

measurements,  as  the  accelerometer  is  able  to  provide  noisy,  yet  accurate  estimates  of 
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pitch  and  roll  via  the  measured  gravity  veetor,  as  long  as  the  vehiele  is  not  aeeelerating. 
When  a  vehiele  is  aeeelerating,  however,  aeeelerometer  measurements  also  inelude 
eomponents  of  linear  and  eentripetal  aeeeleration  that  are  not  individually  measurable.  By 
making  eertain  assumptions  about  the  aeeelerometer  measurements  and  introdueing 
information  from  the  GPS,  better  piteh  and  roll  estimates  ean  be  ealeulated  and  used  to 
eorreet  the  gyro  measurements. 

The  first  measurement  model  (MMl)  investigated  for  this  thesis  estimates  roll  and 
piteh  by  manipulating  the  aeeelerometer  measurement  model  as  shown  in  Equation  (4.6) 
to  direetly  ealeulate  roll  and  piteh  angles.  This  approaeh  is  detailed  in  [20]  and  [21]. 

First,  it  is  assumed  that  the  Coriolis  term  is  negligible,  redueing  the  aeeelerometer 
model  to 


f  d  \ 

F'’=R^  —V‘  -R^g'.  (6.20) 

ydt 

Making  the  assumption  that  the  GPS  measurements  are  obtained  in  the  inertial 
frame,  the  LTP  aeeelerations  are  ealeulated  by  differentiating  the  GPS  veloeities.  The 
north  and  east  veloeity  eomponents  are  ealeulated  by 


V‘  =  cos(i//) 
V‘  =  Vj.  sin(^/^) 


(6.21) 


where  y/  is  the  GPS  measured  eourse  over  ground  and  Vj.  is  the  GPS  measured  speed  of 


the  vehiele. 


The  differentiation  is  earried  out  in  simulation  using  the  3’^‘*  order  filter  deseribed 
in  Equation  (4.3)  and  depieted  in  Figure  12  in  Chapter  IV.  Defining  the  GPS  aeeeleration 

veetors  as  ^  a^p^  ^  z]  ’  Equation  (6.20)  ean  be  rewritten  as 


F'’  =  R’;[A,p,-g').  (6.22) 

The  rotation  matrix  from  FTP  to  body  frame  is  then  manipulated  to  define  a  new 
veetor  =  [r  r 
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F^=R,ReRMoPS-s‘)  =  R,Re 


aGPs,x  cos{i//)  +  a^ps,y  sin(r) 

-«Gra,x  sin  (^^ )  +  a^ps,y  cos 
^GPS,z  ^  IIS' II 


■  c9 

0 

-s9~ 

F"  = 

s(ps9 

c(p 

c9s(p 

c(ps9 

-S(p 

c(pc9 

_ 

(6.23) 


Since  course  over  ground,  y/  ,  is  an  output  of  the  GPS,  an  analytical  solution  can 
be  found  to  solve  for  piteh  and  roll.  Piteh  is  first  determined  by  manipulating 

=r,cos{e)-r^sm{6)  (6.24) 

from  the  first  row  of  Equation  (6.23)  to  solve  for  9,  whieh  after  some  manipulation 
yields 


9  =  tan 


-r. 


(6.25) 


Using  this  piteh  value,  roll  ean  then  be  determined.  The  second  row  from 
Equation  (6.23)  can  be  rewritten  to 


where 


Fy  =  r,sin(6')sin(^)  +  rj,  cos(^3)  +  r^  cos(6')sin(^) 
=Usin(^)  +  UCOs(^) 

G  =';sin(^)  +  i;cos(^) 

Solving  Equation  (6.26)  for  roll  yields 


(6.26) 


(6.27) 


(p  =  tan 


\  y  ) 


(6.28) 


The  measurement  input  veetor  for  attitude  estimation  ean  now  be  formed  as 
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(F'f-F 

¥  GPS 

where  the  sign  in  front  of  the  square  root  for  pitch  and  roll  is  determined  by  the  reference 
frame  used.  For  the  North-East-Down  LTP  frame,  the  signs  are  as  shown. 

This  method  provides  good  estimates  of  pitch  and  roll,  but  loses  accuracy  in  roll 
estimation  when  the  Coriolis  term  is  large,  which  happens  during  sharp  turns.  If  the 
assumption  is  made  that  angle  of  attack  a  =  0  and  sideslip  =  0  then  it  follows  that  the 
velocity  in  the  body  frame  can  be  simplified  to 

u  Fy.  cos(a)sin(y0)  Vj. 

¥”=  V  =  V^smi/3)  =  0  .  (6.30) 

w  Fy  sin(a)cos(y0)  0 

Adding  the  simplified  Coriolis  force  term  back  into  the  accelerometer 
measurement  model  yields: 


(6.31) 


The  calculation  for  pitch  angle  remains  the  same  as  Equation  (6.25),  but  for  roll 
the  calculation  becomes: 


Fy  =  sin  (6*)  sin  (^)  -l-  cos  (^)  -l-  cos  (6*)  sin  (^)  -l-  Vj-r 
Fc,y  =  (K  -  Kr)  =  Vg  sin (^)  +  cos (^) 
where  the  Coriolis  term  is  absorbed  into  the  accelerometer  term  as 


(6.32) 
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Finally,  Equation  (6.28)  is  adjusted  by  substituting  in  ^  for  yielding; 


(p  =  tan  ' 


V,  ±  F,.,  +'■» 


(F) 


-F 


The  new  measurement  veetor  for  MMl  then  beeomes 


-AHRS,1 


(P 

e 

w 


tan 


tan  ' 


■r" 

'e 


{FfF 

¥  GPS 


(6.33) 


(6.34) 


Figures  20  and  21  illustrate  the  aeeuraey  of  MMl  with  the  added  Coriolis  term  as 
measured  from  Condor  simulation  data.  Without  the  added  term,  the  roll  measurements 
overshoot,  whieh  requires  judieious  tuning  of  the  noise  matrix  during  tight  turns.  To 
avoid  having  to  develop  an  adaptive  tuning  teehnique,  the  Coriolis  term  method  is  used  in 
MMl  for  subsequent  estimator  testing. 

Figure  20  eompares  the  true  roll  angle  value  from  Condor  with  roll  angles 
eomputed  both  with  and  without  the  Coriolis  term  added.  Sinee  the  addition  of  the 
Coriolis  term  as  previously  defined  has  no  effeet  on  piteh,  only  one  measurement  is 
eompared  against  Condor’s  true  piteh  angle  values. 
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Measurement  Model  1,  Roll  Comparison 


Figure  20.  Roll  measurements  as  approximate  from  MMl . 


0.4  r 


Measurement  Model  1,  Pitch  Comparison 


Measurement 

Truth 

2(j  Std  Deviation 


60  80 
Time  [s] 


140 


Figure  21 .  Pitch  measurements  as  approximate  from  measurement  model  1 . 
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The  standard  deviation  of  the  measured  piteh  and  roll  values  was  ealeulated  by 
the  general  standard  deviation  equation; 

J1  ^ 

(6.35) 

where  the  mean  //  is  assumed  to  be  the  true  value  of  piteh  or  roll  at  eaeh  time  step. 


This  yields  piteh  and  roll  standard  deviations  of  =  0.03  rad  and  -  0.06  rad. 

The  2(j  values  for  piteh  and  roll  are  plotted  in  Figures  20  and  21  and  show  that  the 
measurement  estimates  generally  fall  within  that  bound.  This  allows  for  piteh  and  roll 
sensor  models  to  be  approximated  as  the  true  values  plus  zero  mean  Gaussian  white 
noise; 


(6.36) 


^  =  (P  +  \,  \-N(0,al) 
d  =  0  +  Vg,  ~A^(0, cr^) 

The  measurement  estimate  that  approximates  roll,  pitch,  and  yaw  measurements 
based  on  the  current  state  estimate  is 


-AHRS,l 


tan  ' 

f  2  (^2^3+ ^0^1  )  ^ 
^2  ^2  ^2  .  ^2 

(p 

Uo -9i  -qi  +^3  j 

0 

= 

sin  ' 

(-2  (^1^3 -4^2)) 

+ 

¥ 

tan  ' 

•^2  '^2  '^2  '^2 

V^O  “*"^1  ~^2  “^3  J 

(6.37) 


The  measurement  noise  matrix  for  this  measurement  model  is  then  defined  as 


R 


AHRS,l 


0  0 
0  crj  0 

0  0  rxj 


(6.38) 


2,  ARHS  Measurement  Model  Two 

Instead  of  trying  to  create  a  better  external  measurement  to  compliment  the  gyro, 
measurement  model  two  (MM2)  utilizes  the  available  states  to  approximate  the 
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accelerometer  output.  As  in  Equation  (4.6),  the  accelerometer  is  assumed  to  have  a  sensor 
model  of 


The  DCM  from  the  LTP  to  body  frame  is  eonstrueted  using  the  quaternions  from 
the  AHRS  state  veetor.  The  LTP  veloeity  and  aeeelerometer  bias  terms  are  aequired  from 
the  INS  module  state  veetor.  The  angular  rates  and  aeeeleration  terms  are  treated  as 
system  inputs  to  the  equations.  The  angular  rates  are  measured  from  the  gyro  with  current 
bias  estimates  and  sidereal  rates  removed.  The  linear  aeeelerations  are  created  by 
differentiating  the  GPS  velocities.  Ineluding  the  yaw  estimate,  the  full  measurement 
model  yields; 


l  +  h„  +v„ 


(6.39) 


'pb- 

R'Kps^[[<-KyR:v‘)-R 

:g‘+L 

^  AHRS, 2 

y  _ 

^2  ^2  ^2  ^2 

V  ^0  ^1  ■  ^2  ”  ^3  J 

+ 

y. 

with  assoeiated  measurement  noise  matrix 

0  0 

0  0 

0 

0 


R 


2,  AHRS 


0 

0 

0 


0 


(6.40) 


(6.41) 


Figures  22-24  show  aeeelerometer  estimates  generated  using  perfeet  state 
estimates,  but  noisy  GPS  aeeeleration  and  gyro  measurements.  All  measurements  were 
obtained  via  Condor  simulation. 
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Figure  22.  Acceleration  X-channel  comparison  using  the  equations  from  MM2. 


Acceleration  Y-channel  Measurement  and  Estimate  Comparison 


Figure  23.  Acceleration  Y-channel  comparison  using  the  equations  from  MM2. 
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Acceleration  Z-channel  Measurement  and  Estimate  Comparison 


Figure  24.  Acceleration  Z-channel  comparison  using  the  equations  from  MM2. 

It  is  clear  from  Figures  22-24  that  the  GPS  acceleration  estimates  and  gyro 
measurements  contain  more  noise  than  the  actual  accelerometer  measurements.  Though 
noisy,  the  mean  of  the  estimates  contain  the  true  value  of  the  acceleration  in  each  channel 
and  are  therefore  a  good  representation  of  the  accelerometer  measurements. 

3,  INS  Measurement  Model 

The  measurement  models  for  the  position  and  velocity  corrections  are 
straightforward  and  do  not  require  any  unique  mechanics.  The  estimates  are  pulled 
directly  from  the  INS  module  state  vector  and  compared  to  the  GPS  output.  The  GPS 
position  and  velocity  measurements  are  assumed  to  be  contaminated  with  additive  zero 
mean  white  noise; 
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Pe 

+ 

t 

_ 1 

(6.42) 


The  PSD  for  the  noise  values  in  Equation  (6.42)  are  loeated  in  Chapter  IV  Table  4 
for  Condor  simulated  data  and  Table  5  for  SEAFOX  II  data.  These  values  are  used  to 


form  the  measurement  noise  matrix 


R 


INS 


(6.43) 


ComNav  Vector  G1  Sensor  Specifications 

Sensor 

Noise  PSD 

GPS  Position 

v^~v(0,(5  m)") 

GPS  Velocity 

v^~v|o,(0.5  m/s")"j 

Table  5.  Noise  eharaeteristies  for  the  SEAFOX  II  GPS  sensor,  after 

[4]. 


D,  EKF  EQUATIONS 
1.  AHRS 

As  diseussed  in  Chapter  III,  the  extended  Kalman  filter  requires  first  order 
derivative  matriees  to  projeet  the  state,  measurement,  and  eovarianee  estimates.  The 
AHRS  proeess  model  is  defined  as 
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X 


AHRS 
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f  AHRS  (  ^ AHRS  ’  ^  ^ 

.v)  = 

■  0 

-CO2 

-(U3 

(Uj 

0 

CO^ 

-®2 

-(U3 

0 

(Uj 

(U3 

-(Uj 

0 

(6.44) 


where  u  is  the  process  (gyro)  measurement  and  v  is  the  process  noise. 

According  to  the  sensor  model  in  Equation  (6.19)  the  gyro  measurement  is: 


CO, 


CO. 


CO. 


(6.45) 


The  state  process  Jacobian  is  formed  by  taking  the  partial  derivative  of  the  state 
differential  equation  with  respect  to  each  state,  linearized  around  its  current  value,  with 
noise  terms  equal  to  their  expected  value  of  zero: 


F 


ARHS,k 


F  = 

^  ARHS,k 

^/ahrs 

arhs 

5  ^ARHS,k  ^  ^ 

^Iarhs 

^Iarhs  ^Iarhs 

^Iarhs 

^/arhs 
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dq^ 

dq^ 

dq^ 

dq^ 

0(3x1) 

0(3x1) 

0(3x1) 

0(3x1) 

0(3x3) 

(6.46) 


The  state  excitation  process  Jacobian  is  formed  by  taking  the  partial  derivative  of 
the  state  process  model  with  respect  to  the  noise  states,  linearized  around  the  current 
value  of  the  state  estimates: 


QL 


ARMS 


^  ARHS.k 


dv 


^  ARMS,  k 


^ARHS.k  ^  ^ 


7x6 


S/a, 


•arms 


dv^ 

0, 


0 


(3x3) 


L. 


(6.47) 


>3)  "(3x3)_ 

The  state  process  and  excitation  Jacobian  calculations  are  performed  using 
continuous  time  equations.  The  EKE  is  run  in  discrete  time,  with  discrete  measurements. 
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therefore  the  state  proeess  and  excitation  matrices  must  be  discretized.  The  following 
method  for  continuous  state  discretization  is  outlined  in  chapter  4  of  [8]  and  utilizes 
matrix  exponentials. 

First,  the  matrix  containing  the  process  and  excitation  matrices  is  formed; 


-Fk  GkQAHRS^k 


"(7x7) 


F 


(6.48) 


where 


a 


ARHS 


0 


0, 


(3x3) 

2 


(3x3) 


(6.49) 


is  the  process  noise  matrix,  and  T  is  the  fdter  time  step  length. 

Next,  the  matrix  exponential  is  performed  using  the  MATLAB  expm  function; 


Y  =  e^ 


-D 

0(7X7) 


(6.50) 


Z)  is  a  dummy  variable,  Qd/^  is  the  discrete  time  state  excitation  matrix,  and  is 
the  discrete  time  state  transition  matrix  at  time  k. 


The  discrete  time  state  excitation  matrix  and  state  transition  matrix  can  be  easily 
extracted  from  Equation  (6.50)  for  use  in  the  EKE  error  covariance  time  propagation 
equation; 


Pk\.=^kP:^l  +  Qdk  (6.51) 

Easily,  the  measurement  process  Jacobians  must  be  formed.  The  Jacobian  of  the 
first  measurement  process  model,  from  Equation  (6.37),  is  linearized  with  respect  to  the 
current  estimates  of  the  states,  and  given  by 
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dz 


AHRS,\ 


AHRS,\,k 


dX 


ARMS 


3z  3z  3z  3z 

^^AHRS,\  ^^AHRS^  '^^AHRS,l  ^^AHRS^ 


dq^  dq, 
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.(6.52) 
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The  second  measurement  model  process,  given  by  Equation  (6.40),  is  linearized 


to  give; 


IT  ez. 

^  AHRS2,k  ^  ^ 


^  AHRS,2,k 


.  (6.53) 
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The  process,  excitation,  and  measurement  Jacobian  matrices  are  extensive.  The 
Appendix  contains  the  MATLAB  code  for  the  EKE  used  in  this  analysis. 

2,  INS 

The  formation  of  the  INS  process  model  follows  the  same  method  used  for 
developing  the  AHRS  EKE  equations.  The  state  process  model  is  defined  from  Equations 
(6.12),  (6.17),  and  (6.18)  as 


^  INS  f  INS  (  ^  INS  ’  ^  A  ) 


E;  =  R[F^)  +  g‘ -2S[co[)v‘ 

k 


(6.54) 


where 


The  Jacobian  of  the  state  process  model  is  then  defined  as 


(6.55) 


F  -  ^fiNs 

INS,k  p  Y 


F  eR 
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The  Jacobian  of  the  state  excitation  matrix  is  defined  as 


(6.56) 
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(6.57) 
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The  discrete  time  propagation  matrices  are  formed  using  the  same  process  from 
the  AHRS  estimator.  The  measurement  process  Jacobian  is  trivial  since  all  the 
measurements  are  assumed  to  contain  additive  white  noise; 


dz 

- 

IT  ^^INS 

^INS  - 

^^INS 

/(6x6)  0(g^3)^ 

(6.58) 


E,  UKF 


Perhaps  the  largest  benefit  of  using  the  unscented  Kalman  filter  is  the  simplicity 
of  the  process  time  propagation  and  measurement  updates.  Instead  of  forming  complex 
Jacobians  to  estimate  the  process  and  measurement  models,  the  differential  equations 
from  Equations  (6.44)  and  (6.54)  are  directly  integrated  using  first  order  Euler  integration 
methods.  The  augmented  state  vectors  for  the  AHRS  and  INS  modules  accommodate  the 
zero  mean  noise  sources  and  are  initialized  according  to  Equation  (3.35)  as: 
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(6.60) 
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.r  (6-61) 

"  INS,0  '^(1x3)  '^(1x6)  _ 

The  standard  UKE  and  SR-UKE  use  2E  + 1  sigma  vectors  to  calculate  the  process 
mean  and  covariance  and  the  SSUKE  and  SR-SSUKE  utilize  L  +  2  as  discussed  in 
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Chapter  III,  where  L  is  defined  as  the  number  of  augmented  states  contained  in  the 
respective  augmented  state  vector.  Each  variant  of  the  UKF  was  built  in  accordance  to 
the  outline  in  Chapter  III,  the  MATLAB  code  for  which  is  available  in  the  Appendix. 
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VII.  DISCUSSION  AND  RESULTS 


A,  OVERVIEW 

In  order  to  determine  the  qualities  of  each  Kalman  filter  covered  in  Chapter  III, 
each  estimator  design  was  test  with  a  common  data  set  captured  in  the  Condor  simulation 
environment.  Five  Kalman  filters  and  two  attitude  measurement  models,  equaling  ten 
total  navigation  estimators  were  evaluated.  Each  estimator  was  initialized  with  identical 
state  and  covariance  estimates,  but  with  individually  tuned  measurement  and  process 
noise  values  that  best  enabled  each  filter  to  track  the  true  value  of  the  estimated  states. 
First,  each  estimator  was  evaluated  based  on  computation  time  of  an  identical  data  set  to 
test  for  filter  efficiency.  To  test  for  accuracy,  each  estimator  was  initialized  with  the  true 
gyro  and  accelerometer  bias  values  and  evaluated  on  convergence  to  the  true  state  values. 
Next,  to  test  for  robustness,  each  estimator  was  initialized  with  arbitrary  gyro  and 
accelerometer  bias  values  and  again  evaluated  on  convergence  to  the  true  state  values. 
Each  estimator  was  then  ranked  by  performance  in  a  series  of  weighted  categories  to 
determine  the  best  overall  design.  Easily,  the  EKE  and  top  ranked  unscented  Kalman 
filter  were  evaluated  against  SEAFOX  II  data. 

B,  FILTER  TUNING 

Perhaps  the  most  time  consuming  portion  of  most  Kalman  filter  design  is  properly 
tuning  the  measurement  and  noise  matrices.  Instead  of  iteratively  evaluating  entire  data 
sets  with  constant  test  values  for  the  noise  and  covariance  matrices,  each  filter  was  tuned 
online  by  adjusting  a  series  of  gains  while  evaluating  their  real  time  effects  on  the  state 
estimates  in  the  Condor  simulation  environment.  The  PSD  noise  values  used  in  the 
Condor  simulations  are  summarized  in  Table  6.  The  AHRS  process  and  measurement 
noise  matrices  for  both  MMl  and  MM2  are  initialized  as: 
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Qahrs  =  diag  ( ctI  ] ) 

^AHRS^  ~  diag{j^ki(7l  k^crl  ^3^^])  (6-62) 


with  INS  estimator  process  and  noise  matrices  initialized  as 


Qm  =  diagi^crl,  cr^, 
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(6.63) 


Noise  Characteristics  for  Simulation  Sensor  Outputs 

Sensors 

Noise  PSD 

Gyro 

cr^^  =(0.01  rad/s)^ 

Accelerometer 

cr^,  =(0.1  rad/s)^ 

Measured  Euler  Angles 

=(0.03  rad)^  ,f7^  =(0.06  rad)^  ,a^  =(0.001  rad)^ 

GPS  Position 

CTp  =  (3  m)^ 

GPS  Velocity 

ay  =(0.3m/s^^  ,cr^  =(0.3m/s^j  ,cr^^  =(0.001m/s^j 

Table  6.  Noise  variance  values  for  process  and  measurement  noise 
in  the  Condor  simulation  environment. 


All  gain  values  in  Equations  (6.62)  and  (6.63)  were  scaled  from  a  value  of  one  to 
the  final  values  listed  in  Table  7.  The  ratio  of  process  noise  to  measurement  noise 
determines  the  level  of  “trust”  in  the  aiding  sensor  measurement  by  roughly  scaling  the 
Kalman  gain  according  to 

Koc^.  (6.64) 

R 
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Large  values  for  measurement  noise  result  in  a  small  Kalman  gain  allowing  the 
proeess  and  proeess  sensors  to  integrate  with  minimal  eorreetion.  Small  values  for 
measurement  noise  inerease  the  Kalman  gain  introdueing  large  eorreetions  to  the  proeess. 
Sinee  the  aided  sensors  are  also  eorrupted  by  noise  and  error,  the  measurement  gain  had 
to  be  earefully  tuned  in  order  to  avoid  over  eorreetion  and  exeess  noise  introduetion  to 
the  system. 


Process  and  Measurement  Noise  Scaling  Factors 


Gain 

EKF 

UKF 

SR-UKF 

SSUKF 

SR-SSUKF 

AHRS  Measurement 
Model  1 

k\ 

1 

0.05 

0.025 

0.05 

1 

ki 

1 

0.005 

0.025 

0.005 

1 

h 

1 

1 

1 

1 

1 

h 

1 

0.001 

0.01 

0.001 

0.01 

kg 

1 

0.001 

0.01 

0.001 

0.01 

kw 

1 

0.001 

0.01 

0.001 

0.01 

ki\ 

1 

0.005 

0.005 

0.005 

0.01 

kn 

1 

0.005 

0.005 

0.005 

0.01 

ki^ 

1 

1 

1 

1 

1 

AHRS  Measurement 
Model  2 

ki, 

1 

0.025 

0.02 

0.25 

5 

ks 

1 

0.025 

0.05 

0.25 

1 

h 

1 

0.01 

0.1 

0.1 

1 

ki 

1 

1 

1 

1 

1 

ku 

1 

0.001 

0.001 

0.0001 

0.001 

ki5 

1 

0.001 

0.001 

0.0001 

0.001 

ki6 

1 

0.001 

0.001 

0.0001 

0.001 

kn 

1 

0.005 

0.005 

0.005 

0.01 

kn 

1 

0.005 

0.005 

0.005 

0.01 

ki9 

1 

1 

1 

1 

1 

Table  7.  Final  proeess  and  measurement  noise  sealing  faetors. 


Comparison  of  sealing  faetors  assoeiated  with  the  EKF  and  UKF  variants  is 
diffieult  as  the  filter  meehanies  are  different.  The  EKF  Kalman  gain  is  direetly  sealed  by 
the  measurement  noise  matrix  whereas  the  UKF  Kalman  gain  is  indireetly  sealed  by  the 
proeess  and  measurement  noise  matriees  whieh  are  ineorporated  into  the  proeess  of  sigma 
point  seleetion.  With  the  exeeption  of  the  SR-SSUKF,  the  remaining  UKF  variants  are  all 
sealed  on  or  near  the  same  order  of  magnitude. 
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It  should  be  noted  that  the  EKF  designed  with  AHRS  measurement  model  2  is  left 
out  of  all  subsequent  analysis  as  EKF(2)  was  completely  unstable  for  all  evaluated  tuning 
gains.  The  instability  was  due  to  the  high  nonlinearity  of  the  measurement  model 
equations. 

C.  FILTER  COMPUTATION  SPEED  COMPARISON 

The  largest  detractor  from  the  unscented  Kalman  filter  and  related  filters  (e.g. 
particle  filter  and  Bayesian  filter)  is  the  number  of  iterations  required  to  compute  a  single 
time  step.  In  general,  the  number  of  UKE  iterations  per  cycle  is  scaled  based  on  the 
number  of  states  being  estimated.  The  standard  UKE  and  SR-UKE  use  weights  that 
require  2L  +  \  iterations,  where  L  is  defined  as  the  number  of  states  plus  the  number  of 
measurement  noise  terms  and  the  number  of  process  noise  terms.  The  SSUKE  and  SR- 
SSUKE  utilize  the  Spherical  Simplex  weight  set  which  only  requires  L+2  iterations  per 
cycle.  The  total  iterations  per  cycle  for  each  estimator  module  are  listed  in  Table  8. 


Module 

UKE  &  SR-UKF 

SSUKF  &  SR-SSUKF 

AHRS  (Model  1) 

27 

15 

AHRS  (Model  2) 

29 

16 

INS 

37 

20 

Table  8.  Number  of  iterations  per  time  step  of  each  UKE  variant  for 
the  given  estimator  design. 


To  evaluate  the  overall  speed  of  each  filter,  an  experiment  was  designed  in 
Simulink  to  run  a  10  second  data  set  from  Condor  through  each  filter.  The  MATEAB 
functions  tic  and  toe  act  as  a  high  accuracy  CPU  stopwatch  and  were  executed  in  the 
simulation  start  function  and  stop  function  callback,  respectively,  to  measure  the  elapsed 
time  for  each  filter.  The  results  from  this  experiment  are  shown  in  Eigure  25. 
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CPU  Processing  Time  Vs.  EKF(1) 
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Figure  25.  Comparison  of  relative  eomputation  time  of  a  10  seeonds  data  set  run  in 

Simulink  against  the  EKF(l)  run  time. 


The  results  shown  in  Figure  25  are  not  surprising  in  that  the  unseented  filters  with 
spherical  simplex  weights  outperformed  those  with  standard  weights  as  the  spherical 
simplex  method  employs  fewer  sigma  points  and  thus  less  iteration  per  time  step.  The 
SR-UKF  and  SR-SSUKF  implementations  were  expected  to  slightly  outperform  their 
counterparts  as  the  linear  algebra  operators  that  form  the  square  root  implementation  are 
more  efficient  than  the  Cholesky  square  root  operator.  This  result  was  true  for  the  normal 
UKF  sigma  point  scheme,  but  not  so  for  the  spherical  simplex. 

A  second  test  for  computational  efficiency  was  run  to  minimize  any 
computational  overhead  that  may  have  come  from  using  Simulink  and  its  discrete  time 
solver.  Each  estimator  was  built  in  the  form  of  a  MATLAB  function  file  and  was  run  in  a 
script  with  a  random  vector  input  to  measure  the  time  of  a  single  time  step,  again  using 
the  tic  and  toe  MATEAB  functions.  Each  estimator  was  run  25  times,  the  times  of  which 
were  normalized  against  the  EKE(l)  run  time  and  shown  in  Eigure  26. 
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Figure  26.  Comparison  of  relative  eomputation  time  for  a  single  iteration  of  eaeh 

estimator  funetion  against  EKF(l). 
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Running  eaeh  estimator  as  a  funetion  eall  in  MATLAB  resulted  in  a  signifieant 
inerease  in  speed  performanee  from  the  unseented  filters.  The  largest  performanee 
inerease  eame  from  UKF(2)  and  SR-UKF(2)  whieh  improved  by  132%  and  138%, 
respeetively.  The  smallest  improvement  eame  from  UKF(l)  with  a  deerease  of  35% 
eomputation  time.  While  it  is  diffieult  to  pinpoint  what  exaetly  differs  between 
MATLAB  and  Simulink  exeeution  of  eertain  funetion  ealls,  what  is  apparent  is  that  it  is 
possible  to  reduee  the  relative  eomputation  time  of  an  unseented  Kalman  filter  to  near 
EKF  speed  depending  on  the  effieieney  of  the  software  used  to  exeeute  the  filter. 

The  MM2  estimators  were  expeeted  to  have  inereased  eomputation  times 
eompared  to  their  MMl  eounterparts  as  there  are  slightly  more  sigma  points  to  ealculate 
and  the  formation  of  the  measurement  estimate  Zj^^rsi  froni  Equation  (6.40)  requires 

multiple  rotation  matriees  and  a  eross  produet. 
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With  the  exception  of  the  SR-SSUKF(l)  estimator,  the  square  root 
implementations  outperformed  their  non-square  root  counterparts  in  Figure  26  by  a  wider 
margin  than  those  in  Figure  25.  This  may  be  due  to  differences  between  MATLAB  and 
Simulink  execution  of  the  QR  decomposition  and  Cholesky  Update  operators. 

D,  ATTITUDE  PERFORMANCE 

Two  experiments  were  carried  out  to  analyze  the  attitude  performance  of  each 
estimator.  For  each  experiment  the  filters  were  fed  the  same  Condor  data  set  and 
initialized  with  the  same  state  and  covariance  estimates.  For  the  first  experiment,  the  gyro 
and  accelerometer  bias  values  were  initialized  to  their  true  values.  For  the  second 
experiment,  the  gyro  and  accelerometer  values  are  initialized  to  zero  and  the  covariance 
was  adjusted  to  reduce  trust  in  the  bias  channels,  as  shown  in  Table  9. 
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State  and  Covariance  Initialization  Values 

Initial  Estimate 

AHRS  Module  Experiment  1 

Attitude 

^(0)  =  [0.9529,0.0055,-0.0172,0.3029] 

Gyro  Bias 

4  (0)  =  [2.4xl0^,-1.0xl0  \2.0xl0^]  rad/s 

Covariance 

(0)  =  '^10 '^10 '^10 '^10 ']) 

Initial  Estimate 

AHRS  Module  Experiment  2 

Attitude 

^(0)  =  [0.9529,0.0055,-0.0172,0.3029] 

Gyro  Bias 

4(0)  =  [0,0,0]  rad/s 

Covariance 

^HRS  (0)  =  ([i  O' ,  1 OM  OM  0-^ ,  1 OM  OM  0-4) 

Initial  Estimate 

INS  Module  Experiment  1 

Position 

(0)^ [3  8047x10", -1.095xl0^1.4180xl0']  m 

Velocity 

E"®"(0)  =  [25,17,0.6]  m/s" 

Accelerometer  Bias 

4(0)  =  [1.5x10  ",-1.5x10  ",1.0x10  "]  m/s" 

Covariance 

4^^(0)  =  J/ag  ([10  ^10^10^10^10  ^  10  ^  10  ^  10  ^  10^®]) 

Initial  Estimate 

INS  Module  Experiment  2 

Position 

(0)^ [3  8047x10", -1.095xl0^1.4180xl0"]  m 

Velocity 

E"®"(0)  =  [25,17,0.6]  m/s" 

Accelerometer  Bias 

4(0)  =  [0,0,0]  m/s" 

Covariance 

4^^(0)  =  t//ag([l0  ^10  ^10  ^10  ^10  ^10  ^10  ",10  ",10  "]) 

Table  9.  Initial  state  and  covariance  estimates  used  initialize  each 

navigation  estimator. 
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As  the  SEAFOX  II  is  natively  able  to  produee  a  well  filtered  estimate  of  heading 
from  its  multiple  GPS  reeeivers,  the  main  focus  of  this  section  will  be  to  evaluate  roll  and 
pitch  estimation.  Figure  27  highlights  each  estimator’s  performance  at  the  peak  of  a  high 
rate  turn  from  the  first  experiment’s  data  set.  The  top  graph  shows  MMl  estimators  and 
the  bottom  section  shows  MM2  estimators. 


The  EKF(l)  is  the  noisier  of  the  MMl  AHRS  estimators,  but  accurately  captures 
the  true  roll  value  within  the  noise.  The  UKF(l)  does  not  distinctly  appear  on  either  plot 
because  it  is  practically  identical  to  the  SSUKF(l)  values  which  is  a  surprising  result  in 
that  the  SSUKF(l)  utilizes  nearly  half  the  sigma  points  as  the  UKF(l)  estimator.  A 
cleaner  distinction  between  normal  sigma  points  and  spherical  simplex  sigma  points  is 
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shown  in  the  SR-UKF(l)  and  SR-SSUKF(l)  where  the  SR-UKF(l)  is  able  to  more 
closely  follow  the  true  roll  value  in  high  rate  turns,  but  the  SR-SSUKF(l)  is  slightly 
smoother.  On  the  bottom  plot  of  Figure  27  UKF(2)  and  SSUKF(2)  perform  the  best  in  the 
turn  nearly  matching  the  true  roll  value,  but  have  the  slowest  convergence  time  at 
initialization  as  shown  in  Figure  28.  The  SR-SSUKF(2)  while  containing  less  variance 
than  the  EKF(l)  is  the  worst  performer  in  the  turn  at  nearly  a  degree  off  for  the  duration 
segment  shown. 

Figure  28  shows  the  convergence  time  of  each  estimator,  again  the  top  plot 
represents  MMl  estimators  and  the  bottom  plot  represents  measurement  MM2. 


Initial  Roll  Angle  Convergence  Comparison,  Bias_  =  Bias, 
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Figure  28.  Roll  angle  estimates  with  MMl  (top),  MM2  (bottom). 
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The  EKF(l),  SR-UKF(l),  and  SR-SSUKF(l)  converge  on  the  true  value 
immediately  whereas  the  UKF(l)  and  SSUKF(l)  converges  to  within  a  standard 
deviation  of  the  roll  angle  as  calculated  from  the  accelerometer  approximation  in  AHRS 
MMl,  after  35  seconds.  Similarly  the  UKF(2)  and  SSUKF(2)  have  the  slowest 
convergence  time  of  the  second  measurement  model  group,  but  the  UKF(2)  is  able  to 
converge  in  10  seconds  where  the  SSUKF(2)  converges  again  at  35  seconds. 

Next,  the  pitch  estimates  from  the  first  experiment  are  shown  in  Figure  29  and  30. 
Figure  29  contains  the  estimates  from  MMl  and  two  in  a  high  rate  turn,  and  Figure  30 
shows  the  pitch  estimate  convergence. 


Figure  29.  Pitch  angle  estimates  during  a  high  rate  turn  with  MMl  (top), 

MM2  (bottom). 
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The  EKF(l)  for  pitch  angle  estimates  has  significantly  higher  variation  than  the 
unscented  filters.  Both  sets  of  estimators  appear  to  give  very  similar  results  for  pitch 
estimation.  The  square  root  filters  contain  slightly  more  variation  than  the  UKF(2)  and 
SSUKF(2)  filters.  The  biggest  difference  between  the  two  measurement  models  for  pitch 
estimation  happens  from  time  170  to  185  seconds  in  Figure  29  where  the  model  one 
estimators  underestimate  the  pitch  and  the  model  two  estimators  overestimate  the  true 
pitch  values.  This  is  due  to  the  assumptions  made  calculating  the  measured  pitch  values 
from  MMl  where  the  Coriolis  term  only  aids  the  roll  measurement  resulting  in  slightly 
underestimated  pitch  moments. 
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A  more  realistic  test  of  each  estimator’s  performance  is  with  unknown  gyro  and 
accelerometer  terms  as  in  most  cases  the  true  accelerometer  and  gyro  biases  and  error  are 
rarely  known  without  expensive  bench  testing.  The  gyro  and  accelerometer  biases  are 
integral  to  the  measurement  and  process  equations  which  can  lead  to  filter  divergence  if 
the  bias  estimates  are  poor  and  the  errors  not  constantly  driven  towards  zero.  Figures  31 
and  32  capture  the  same  roll  angle  highlights  from  Figures  27  and  28  to  show  a 
comparison  of  robustness. 
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Figure  32.  Roll  angle  estimates  for  MMl  estimators  when  gyro  and  accelerometer 

biases  are  unknown  (top),  MM2  (bottom). 


Each  filter  displayed  slightly  less  accuracy  than  in  the  known  bias  case,  but 
surprisingly  the  SR-UKF  which  for  the  known  bias  case  performed  the  best,  for  both 
measurement  models  performed  the  worst  and  slowly  diverged  in  the  SR-UKF(2) 
implementation. 

The  pitch  estimates,  shown  in  Figures  33  and  34,  follow  a  similar  trend  to  the  roll 
estimates  in  that  for  the  most  part  all  the  estimators  are  slightly  less  accurate,  but  the  SR- 
UKF  for  both  measurement  models  performs  the  worst. 
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Figure  33.  Pitch  angle  estimates  for  MMl  estimators  when  gyro  and  accelerometer 
biases  are  unknown  during  a  high  rate  turn  (top),  MM2  (bottom). 


In  Figure  33,  the  SR-UKF(l)  pitch  estimates  initially  diverge,  then  slowly  begin 
to  converge  during  the  high  rate  turn  which  indicates  that  the  bias’  are  not  diverging  for 
the  entire  duration  of  the  data  set. 
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Initial  Pitch  Angle  Convergence  Comparison,  Bias^  =  [0] 


EKF(1) 

UKF(1) 

-  SR-UKF(I) 

-  SSUKF(1) 

SR-SSUKF(1) 

-  Truth 

Figure  34.  Pitch  angle  estimates  for  measurement  model  2  estimators  when  gyro  and 
accelerometer  biases  are  unknown  (top),  MM2  (bottom). 


The  relatively  poor  performance  of  the  SR-UKF  estimator  and  the  difference 
between  the  two  variants  is  explained  best  by  viewing  the  estimated  bias  error  in  Figure 
35.  The  AHRS  module  for  both  measurement  models  largely  depends  on  accurate  gyro 
bias  estimation.  Figure  35  shows  the  root  mean  squared  (RMS)  error  of  the  residual 
between  the  true  gyro  bias  and  estimated  bias. 
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SR-UKF(1)  Gyro  Bias  RMS  Error 


03 


SR-UKF(2)  Gyro  Bias  RMS  Error 


Figure  35.  SR-UKF  gyro  bias  estimates  for  experiment  2. 

The  gyro  bias  error  for  SR-UKF(l)  is  slowly  decreasing  throughout  the  data  set, 
but  remains  an  order  of  magnitude  off  which  explains  the  constant  offset  seen  in  the  pitch 
and  roll  estimates.  The  bias  errors  for  SR-UKF(2)  are  two  orders  of  magnitude  off  in  the 
roll  and  yaw  rate  chaimels  and  growing  which  explains  why  the  roll  estimate  constantly 
diverged.  The  pitch  rate  channel  bias  error  of  SR-UKF(2)  remains  at  a  low  value  on  the 
same  order  of  magnitude  as  the  actual  bias  which  explains  why  it  was  able  to  track  pitch 
nearly  as  well  as  the  other  estimators. 

The  average  RMS  error  for  pitch,  roll,  and  yaw  for  both  experiments  is 
summarized  in  Figure  36.  The  EKF(l),  UKF(l),  SSUKF(l),  and  SR-SSUKF(l) 
estimators  were  the  most  robust,  gaining  the  least  amount  of  error  from  arbitrary  bias 
initialization. 
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Figure  36.  Summary  of  averaged  RMS  error  for  attitude  estimation  for  both 

experiments. 


The  EKF(l),  while  robust  in  comparison,  had  the  highest  RMS  values  for  pitch 
and  yaw,  and  second  highest  for  roll  when  the  bias  was  well  known.  The  SR-SSUKF(l) 
had  the  best  overall  performance  claiming  the  lowest  RMS  error  for  pitch,  roll,  and  yaw 
when  the  bias  was  unknown,  and  second  lowest  when  the  bias  values  were  known  at 
initialization. 
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E. 


POSITION  AND  VELOCITY  PERFORMANCE 


For  Condor  simulation  the  INS  process  model  equations  were  simplified  by 
assuming  a  local  tangent  plane  frame  of  reference  and  no  sidereal  rate,  which  created  a 
more  linear  process.  The  measurement  model  shared  by  all  the  estimators  assumes  the 
GPS  position  and  velocity  measurements  are  corrupted  with  Gaussian  white  noise,  but  no 
other  errors,  which  in  the  case  of  the  Condor  simulation  environment  is  true.  The  RMS 
velocity  errors  from  the  known  and  unknown  bias  experiments  are  shown  in  Figure  37. 


Overall,  the  RMS  values  for  each  filter  are  near  identical  which  is  to  be  expected 
for  the  relatively  clean  aiding  GPS  measurements.  The  unknown  bias  case  for  SR- 
UKF(2)  is  the  worst  performing  estimator  which  can  be  explained  by  both  the  poor 
attitude  estimates  the  INS  module  received  and  the  relatively  large  accelerometer  bias 
errors  shown  in  Figure  38. 
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Figure  38.  Accelerometer  RMS  bias  errors  for  SR-UKF(2) 
with  arbitrary  initial  bias  values. 
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Despite  the  relatively  similar  accuracy  in  velocity  estimation  there  is  a  larger 
difference  in  position  estimation  shown  in  Figure  39.  The  position  estimates  are  purely  a 
function  of  the  current  velocity  and  aiding  sensor  measurement.  The  aircraft  in  Condor 
simulation  soars  at  near  30  m/s,  so  small  errors  in  velocity  can  quickly  cause  the  position 
estimate  errors  to  grow. 
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None  of  the  position  estimate  errors  grew  out  of  control  which  was  largely  a 
function  of  proper  filter  tuning.  The  lack  of  accurate  bias  and  attitude  estimation  in  the 
case  of  the  SR-UKF  had  little  effect  on  position  quality  as  the  estimator  was  able  to 
overcome  the  error  by  relying  more  heavily  on  the  GPS  measurement. 

F.  ESTIMATOR  RANKING  AND  SELECTION 

To  determine  the  best  estimation  filter  for  this  navigation  system  design,  each 
estimator  was  ranked  based  on  a  one-to-nine  scale  base  on  the  average  RMS  error 
(Figures  36,  37,  and  39)  and  the  relative  computation  times  (  Figures  25  and  26).  Figure 
40  is  a  radar  plot  of  the  estimator  rankings  in  each  major  category;  accuracy,  robustness, 
and  efficiency. 
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Estimator  Comparison 


Robustness 
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EFK(1) 

UFK(1) 
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Figure  40.  Radar  plot  of  filter  evaluation  eriteria,  score  on  a  scale  of  one  to  nine, 

where  nine  is  the  best  score. 


The  first  experiment,  where  the  gyro  and  accelerometer  bias  was  initialized  with 
the  true  values,  represents  an  evaluation  of  accuracy  and  takes  into  account  attitude, 
position  and  velocity  RMS  values.  An  evaluation  of  robustness  is  determined  based  on 
the  average  RMS  values  from  the  second  experiment  where  bias  values  where  initialized 
to  zero  and  also  takes  into  account  attitude,  position,  and  velocity  errors.  The  efficiency 
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category  takes  into  account  the  relative  speed  of  each  UKF  in  comparison  to  the  EKF 
runtime. 

In  general,  the  SR-UKF  variants  were  the  most  accurate,  followed  by  the  standard 
UKF  and  SSUKF  variants,  respectively,  while  the  EKF  ranked  last.  Moreover,  filters 
based  on  MMl  were  typieally  more  aeeurate  than  those  based  on  MM2.  Surprisingly,  the 
SR-SSUKF  did  not  follow  the  same  trend.  This  filter  appeared  to  be  especially  sensitive 
to  the  choice  of  measurement  model,  as  SR-SSUKF(l)  was  the  most  aeeurate  filter  of  all, 
while  SR-SSUKF(2)  was  the  least  accurate  UKF  variant.  The  EKF  last  plaee  ranking  was 
expected  since  it  relies  on  the  linearization  of  the  highly  nonlinear  measurement 
equations  of  the  AHRS  module. 

A  eompelling  result  from  these  experiments  is  that  the  SR-SSUKF(l)  was  both 
the  most  aeeurate  and  most  robust  filter  tested.  In  general  the  estimators  utilizing  AHRS 
MMl  out  performed  the  MM2  variants.  SR-UKF(2)  was  the  least  robust  as  the  errors  for 
attitude  diverged  for  the  duration  of  the  data  set  in  experiment  two.  The  EKF,  while  near 
the  bottom  of  the  pack  fared  well  in  that  the  RMS  errors  saw  little  change  with  arbitrary 
bias  initialization,  but  the  relative  estimation  aecuracy  was  limited  by  the  high  variation 
in  each  estimate. 

With  respect  to  efficiency,  the  EKF  surpasses  all  the  UKF  variants.  As  previously 
discussed,  the  UKF  is  dominated  by  the  number  of  iterations  per  time  step  leading  to  the 
high  ranking  of  the  spherieal  simplex  variants  and  the  low  ranking  of  the  normal 
weighted  UKF. 

Direct  comparison  is  an  important  consideration  when  evaluating  the  qualities  of 
each  filter  variant.  For  the  SEAFOX  II  application  however,  it  is  more  important  that  the 
eandidate  estimator  perform  well  in  speeific  categories.  Table  10  lists  the  subjective 
design  weights  whieh  were  used  as  multipliers  against  the  relative  ranking  of  eaeh  filter. 
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Ranking  Weights 

Parameter 

Accuracy  Experiment 
Weights 

Rohustness  Experiment 
Weights 

Roll 

9 

10 

Pitch 

9 

10 

Yaw 

3 

4 

Xn 

5 

6 

Ye 

5 

6 

Vn 

4 

5 

Ve 

4 

5 

Time 

10 

10 

Table  10.  Subjective  weights  assigned  to  each  evaluated  estimation  category. 


The  weights  scale  from  1-10,  where  ten  is  the  most  critical  and  1  is  the  least 
critical.  The  COMNAV  GPS  system  onboard  SEAFOX  II  provides  accurate  position, 
speed,  and  heading  estimates  at  a  frequency  of  20  Hz,  which  exceeds  the  needs  of  the 
ATLAS  Sonar.  What  is  not  provided  is  accurate  pitch  and  roll  estimates,  which  are 
critical  parameters  for  sonar  correlation  accuracy.  Lastly,  the  PC- 104  installed  on 
SEAFOX  II  will  be  running  multiple  programs  for  various  systems;  therefore  the 
efficiency  of  the  estimator  is  very  important.  The  results  for  the  weighted  comparison  are 
shown  in  Figure  41. 
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The  results  for  aeeuraey  are  slightly  shuffled  to  highlight  the  filters  with  the  best 
attitude  estimation.  These  results  eompare  direetly  with  the  RMS  error  plot  previous 
shown  in  Figure  36  where  the  MM2  variants  of  eaeh  estimator  slightly  outperform  their 
model  one  eounterparts  with  the  exeeption  of  the  SR-SSUKF(l)  whieh  outperforms  them 
all.  The  results  for  robustness  saw  no  ehange  as  the  most  robust  filters  provided  the  best 
attitude  estimates.  The  effieieney  eategory  also  saw  not  ehange  as  the  eategory  was 
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simply  scaled  by  the  weighting  factor.  Lastly,  the  weighting  ranking  of  each  category  was 
summed  for  each  filter  to  provide  an  overall  score  where  the  lowest  score  is  the  best 
performing  filter  within  the  weighting  scheme  from  Table  10.  The  results  for  total  score 
are  shown  in  Figure  42. 
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Figure  42.  Weighted  estimator  score  based  on  RMS  errors  and  design  weights.  Lowest 

score  is  best  performing  filter. 


Despite  the  top  spot  for  computational  efficiency,  the  EKF  was  near  the  bottom  of 
the  pack  for  the  accuracy  and  robustness  category.  In  this  navigation  estimator  design,  the 
EKF  performed  well  for  velocity  and  position  estimation,  but  those  performance 
categories  were  discounted  in  comparison  to  the  need  for  robust  and  accurate  attitude 
estimation.  With  the  best  weighted  and  un-weighted  accuracy  and  robustness  as  well  as 
efficient  computational  complexity,  the  SR-SSUKF(l)  is  clearly  the  best  choice  for 
implementation  onboard  the  SEAFOX  11. 
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G. 


SEAFOX  II  ESTIMATION  COMPARISON 


For  this  last  experiment,  data  was  collected  from  sea  trial  maneuvers  on  the 
Monterey  Bay.  The  portion  of  data  used  to  compare  the  EKF(l)  and  SR-SSUKF(l)  was 
captured  during  zig-zag  and  turning  circle  maneuvers  which  provided  relatively  large 
acceleration  in  the  turns.  Figure  43  compares  position  estimates  and  highlights  the 
portion  of  the  data  where  the  SEAFOX  II  was  executing  constant  rudder  turning  circles. 


Figure  43.  EKF(l)  and  SR-SSUKF(l)  position  estimate  comparison 

during  turning  circle  maneuvers. 


On  the  day  on  which  this  particular  data  set  was  obtained,  the  primary  GPS 
receiver  was  down  for  maintenance  and  the  secondary  receiver  provided  the  position, 
speed,  and  course  over  ground.  This  particular  receiver  updates  at  5  Hz,  compared  to 
the  normal  20  Hz  output  by  the  primary  receiver.  Both  EKF(l)  and  SR-SSUKF(l)  were 
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able  to  maintain  accurate  position  estimates.  The  SR-SSUKF(l)  was  able  to  track 
slightly  closer  to  the  GPS  measurement.  North  and  east  velocity  components  are  shown 
in  Figure  44. 


SEAFOXII:  North  Velocity  Component  Comparison 


Time  [s] 


Figure  44.  EKF(l)  and  SR-SSUKF(l)  velocity  estimate  comparison  during  turning 

circle  maneuvers. 


The  EKF(l)  in  Eigure  44  tracks  closer  to  the  GPS  velocity  measurement  than  the 
SR-SSEiKE(l),  but  this  is  mainly  due  to  the  tuning  of  the  SR-SSEiKE(l)  measurement 
noise  matrix.  By  forcing  the  SR-SSEiKE(l)  to  more  closely  mimic  the  GPS  velocity 
measurement,  the  position  estimate  degraded.  Slightly  increasing  the  velocity  channel 
measurement  noise  gains  actually  increased  the  position  estimation  accuracy  of  the  SR- 
SSEiPK(l)  without  introducing  excess  variation. 
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Attitude  estimation  for  SEAFOX  II  is  the  most  eritieal  eomponent  of  this 
partieular  navigation  system  design  as  its  sensors  do  not  directly  measure  roll  angles.  The 
comparison  plots  in  Figures  45  and  46  show  the  pitch  and  roll  estimates  of  the  SEAFOX 
II  during  the  turning  circle  maneuver.  Both  estimators  are  compared  against  the  unfiltered 
gyro  only  solution  of  pitch  and  roll.  For  the  short  duration  under  consideration,  the 
SEAFOX  II  military  grade  ring  laser  gyro  provides  a  highly  accuracy  approximation  of 
pitch  and  roll  values. 


SEAFOX  II:  Roll  Angle  Comparision 


Figure  45.  EKF(l)  and  SR-SSUKF(l)  roll  estimates  compared  against  unfiltered  gyro 

integration. 
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SEAFOXII:  Pitch  Angle  Comparision 


Time  [s] 

Figure  46.  EKF(l)  and  SR-SSUFK(l)  pitch  estimates  compared  against  an  unfiltered 

gyro  solution. 

The  large  values  of  roll  and  pitch  may  at  first  seem  excessive  for  a  boat  operating 
near  shore,  but  there  were  significant  surface  wave  and  swell  action  on  the  day  this 
particular  data  set  was  gathered.  The  EKF(l)  and  SR-SSElKE(l)  produced  near  identical 
results  for  pitch  and  roll  estimation.  Both  filters  appear  to  slightly  overestimate  the  roll 
values  near  the  peaks  in  Eigure  45,  but  provide  good  estimates  of  pitch  in  comparison  the 
to  the  unfiltered  gyro  solution. 

Erom  these  plots  it  is  apparent  that  there  is  little  difference  in  the  overall  accuracy 
between  the  EKE(l)  and  SR-SSElKE(l)  when  considering  low  speed  turning  maneuvers 
on  the  water.  The  EKE  in  general  is  a  tried  and  true  filter  that  was  expected  to  perform 
well  for  this  navigation  estimation  task.  By  using  the  local  tangent  plane  frame  of 
reference,  the  navigation  equations  were  simplified  resulting  in  a  more  linear  process 
model.  The  two  measurement  models  tested  were  both  nonlinear,  but  the  second  model 
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which  estimated  the  aecelerometer  measurements  was  too  nonlinear  for  the  EKF, 
resulting  in  filter  divergenee.  The  unscented  Kalman  filters,  on  the  other  hand,  are 
designed  to  address  the  problem  of  nonlinear  estimation  by  propagating  an  iterative  set  of 
points  through  the  nonlinear  process  to  approximate  the  true  probability  distribution  and 
recover  its  mean  and  covariance.  This  proeess,  while  accurate,  comes  at  a  high 
computational  cost  and  may  be  too  high  for  some  systems.  In  an  effort  to  reduce  the 
computational  burden  associated  with  the  UKF  process,  the  navigation  estimator  was 
split  into  two  modules,  reducing  the  number  of  states  per  estimator  and  therefore 
redueing  the  number  of  iterations  required  by  the  UKF.  For  the  navigation  estimator 
designed  here,  the  benefits  of  the  EKF  are  aeeuraey  and  robustness  on  par  with  the 
unseented  Kalman  filters,  but  with  less  eomputational  burden.  Detracting  from  the  EKF  is 
the  requirement  to  eompute  large  Jaeobian  matrices  of  partial  derivatives,  a  potentially 
large  source  of  error  during  software  implementation  as  the  manual  coding  of  these 
equations  is  extensive.  The  SR-SSUKF  operates  at  a  low  computational  cost  in 
comparison  to  other  UKF  variants,  but  still  requires  nearly  double  the  computation  time 
of  the  EKF.  Perhaps  more  importantly,  the  SR-SSUKF  provides  equal  or  better  aeeuraey 
than  the  EKF  with  more  robustness,  but  does  not  require  coding  or  computing  a  single 
derivative  matrix,  making  it  easier  to  implement  in  software  than  the  EKF. 
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VIII.  CONCLUSION  AND  RECOMMENDATIONS 


A,  CONCLUSION 

The  SEAT OX  II’s  proprietary  architecture  initially  limited  its  ability  to  perform 
experimental,  fully  autonomous  operation,  thereby  relegating  its  utilization  a  chase  and 
recover  boat  for  other  autonomous  systems.  In  order  to  build  autonomous  capabilities 
into  the  SEAFOX  II  and  better  support  the  installation  of  the  bow  mounted  ATE  AS 
sonar,  the  proprietary  remote  control  systems  onboard  were  replaced  with  an  open 
architecture  network  of  sensors,  controllers,  and  computers.  While  the  SEAFOX  II  was 
capable  of  producing  the  standard  maritime  navigation  aids  of  heading,  speed,  and 
position,  it  did  not  have  an  ability  to  produce  accurate  pitch  and  roll  estimates  the 
ATEAS  sonar  requires  to  produce  accurate  sonar  maps.  To  meet  this  need,  the  extended 
Kalman  filter  and  four  variants  of  the  unscented  Kalman  filter  were  tested  in  a  high 
fidelity  simulation  environment  to  determine  the  navigation  estimator  with  the  best 
combination  of  accuracy,  robustness,  and  computational  efficiency.  In  an  effort  to  reduce 
computational  complexity  given  that  the  SEAFOX  II  has  limited  computing  resources, 
the  navigation  estimator  was  separated  into  two  modules,  one  estimating  vessel  attitude 
(AHRS)  and  the  other  estimating  position  and  velocity  (INS).  Two  complex  nonlinear 
measurement  models  were  investigated  to  compliment  the  attitude  estimator.  The  first 
measurement  model  provided  instantaneous  estimates  of  pitch  and  roll  by  combining 
measurements  from  the  accelerometer,  gyro,  and  acceleration  estimates  based  on 
measured  GPS  velocity.  The  second  measurement  model  recreated  accelerometer 
measurements  by  fusing  the  AHRS  and  INS  state  estimates  with  estimated  GPS 
accelerations.  Both  models  provided  accurate  attitude  estimation,  but  the  second  model 
proved  too  nonlinear  for  the  extended  Kalman  filter  to  converge.  After  extensive 
simulation  and  testing  the  square  root  spherical  simplex  unscented  Kalman  filter  (SR- 
SSUKF)  combined  with  the  AHRS  measurement  model  that  approximated  pitch  and  roll 
measurements  exhibited  the  best  combination  of  accuracy,  robustness,  and  efficiency. 
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The  SR-SSUKF  estimator  was  then  eompared  against  the  EKF  for  evaluation  of 
SEAFOX  II  data.  While  the  SR-SSUKF  signifieantly  outperformed  the  EKF  in  the 
simulation  environment,  the  two  estimators  provided  near  identieal  estimation  results 
when  evaluating  aetual  sea  trial  data. 

In  eonelusion,  for  the  problem  of  navigation  state  estimation  on  a  slow  moving 
small  boat  operating  in  loeal  waters,  the  extended  Kalman  filter  provides  adequate 
attitude,  position,  and  veloeity  estimates  with  relatively  low  eomputational  overhead.  In 
simulation  with  highly  dynamie  maneuvers,  the  EKF  eonsistently  underperformed  in 
eomparison  to  nearly  all  the  unseented  Kalman  Filters  for  aeouraey  and  robustness.  The 
SR-SSUKF  on  the  other  hand  proved  to  be  a  powerful  tool  that  provided  equal  to  or 
better  aeeuraey  than  the  EKF  in  both  low  and  high  dynamie  maneuvers  at  a  relatively  low 
eomputational  eost  with  the  added  benefit  of  software  implementation  simplieity.  The 
SR-SSUKF  is  highly  reeommended  for  use  onboard  the  SEAFOX  II. 

B,  RECOMMENDATIONS  FOR  FUTURE  WORK 

The  Kalman  filter  estimators  provided  good  navigation  state  estimates  over  short 
time  periods,  but  were  not  tested  over  long  periods  of  time.  It  is  reeommended  that  eaeh 
estimator  be  tested  over  longer  durations  to  insure  that  the  bias  estimates  do  not  diverge, 
eausing  the  entire  states  estimate  to  diverge.  While  both  the  EKF  and  SR-SSUKF  tested 
aetual  SEAFOX  II  data,  neither  estimator  had  been  implemented  onboard  SEAFOX  IFs 
PC- 104  hardware  for  real-time  at-sea  evaluation. 

The  ATFAS  sonar  will  provide  a  unique  opportunity  to  eombine  path  generation 
and  path  following  algorithms  with  live  sonar  data  for  submerged  obstaele  avoidanee, 
target  deteetion,  and  target  following.  The  REMUS  100  AUV  eould  be  used  as  a  target 
for  the  SEAFOX  II  to  traek  and  follow.  This  would  be  a  diffieult  navigation  task  as  the 
ATFAS  sonar  has  a  limited  field  of  view  and  range,  meaning  the  SEAFOX  II  would  need 
to  navigate  effieiently  to  keep  the  REMUS  within  the  limits  of  the  ATFAS  sonar’s 
deteetion  region. 
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The  UKF  proved  itself  an  accurate  state  estimator,  but  it  can  also  be  modified  for 
use  in  parameter  estimation.  The  SEAT OX  II  is  effectively  a  black  box  in  terms  of 
steering  and  speed  model  system  identification.  Producing  an  accurate  model  of  the 
SEAFOX  IPs  steering  and  speed  dynamics  would  prove  invaluable  in  the  creation  of 
advance  rudder  and  throttle  controllers.  Along  the  same  lines,  adaptive  controllers  could 
be  developed  that  forego  the  need  for  any  system  identification  modeling.  This  would  be 
especially  useful  as  the  SEAFOX  II  often  operates  in  rough  coastal  waters  where  the 
large  wave  action  is  difficult  to  remove  from  rudder  and  engine  feedback  controllers. 
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APPENDIX:  MATLAB  CODE 


A.  OVERVIEW 

Contained  in  the  subsequent  sections  is  the  code  used  to  evaluate  each  Kalman 
filter  in  this  thesis.  The  MATLAB  code  is  designed  as  a  function  and  intended  for  use  in 
Simulink  in  discrete  time  with  fixed  intevals  of  100  Hz.  The  code  was  set  up  to  enable 
either  Condor  data  filtering  or  SEAT OX  II  filtering  which  changes  certain  assumptions 
about  the  LTP  frame,  specifically  the  inclusion  of  the  sidereal  rate  in  the  SEAFOX  II 
implementations.  To  toggle  between  data  types,  the  last  function  input  (u)  must  be 
toggled  a  1  for  Condor  data  or  a  2  for  SEAFOX  II  data.  The  code  was  implemented  in 
Simulink  via  an  Interpolated  MATLAB  function  block. 


B.  EXTENDED  KALMAN  FILTER  MATLAB  CODE 


1,  Measurement  Model  One  Implementation 

function  X_HAT  =  EKFl (u) 

%%  Extended  Kalman  Filter  for  Navigation  State  Estimation 
%  Measurement  Model  1 
%  LT  Steven  Terjesen 
%  September  2014 

%  This  estimator  is  built  for  use  in  MATLAB  Simulink.  There  are  38 
inputs 

%  required  and  can  be  run  in  Simulink  with  an  "Interpolated  MATLAB 
%  Function"  block.  The  first  13  inputs  are  the  vehicle  data:  Course 
Over 

%  Ground,  Speed  Over  Ground,  Accelerometer  Measurements,  Gyro 
Measurements , 

%  GPS  Measurements  (In  LTP  XNorth-YEast-ZDown) ,  Heading  Measurement, 
and 

%  Vertical  Velocity  (zeroed  out  for  SEAFOXII  data) .  Inputs  14  through 
34 

%  are  the  Process  Noise  and  Measurement  Noise  diagonal  elements.  These 
%  elements  are  not  hard  coded  to  allow  for  easier  tuning.  Inputs  35 
%  through  37  are  the  N-E-D  accelerations  in  the  LTP  frame.  Input  38  is 
a 

%  toggle  for  selecting  whether  the  data  is  from  Condor  or  SEAFOX  II. 


%%  System  Inputs 
%  Measurement  inputs 
CoG  =  u ( 1 ) ; 
speed  =  u (2 ) ; 

fx  =  u(3);  fy  =  u(4);  fz  =  u(5); 
p  =  u(6);  q  =  u(7);  r  =  u(8); 


%GPS  Course  Over  Ground 
%GPS  Speed  Over  Ground 
%IMU  Accelerations 
%IMU  Angular  Rates 
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N_sat  =  u(9);  E_sat  =  u(10);  D_sat  =  u(ll); 
heading  =  u ( 12 ) ; 

Vd  sat  =  u  (13) ; 


%  Process  and  Measurement  Noise  Matrices 
R  =  diag (u (14 : 16) ) ; 

R2  =  diag (u ( 17 : 22 ) )  ; 

Qv_C  =  diag (u (23 : 25) )  ; 

Condor 

Qv_S  =  diag (u (23 : 2 8 ) ) ; 

SEAFOX 

Qv2_C  =  diag (u (29 : 31) ) ; 

Condor 

Qv2_S  =  diag (u (2 9 : 34 ) ) ; 

SEAFOX 

%  GPS  Accelerations 
ax_gps  =  u(35);  ay_gps  =  u(36);  az_gps  =  u(37);  %GPS  Accelerations  as 

%  calculated  from  3rd 
%  Order  Filter 

SIM  =  u(38);  %Condor(l)  SEAFOX (2) 

%%  Initialization 

persistent  x  hat  x  hat2  P  P2  g  dt  ii  jj  HI  H2  H3  H4  HeadingO  psiO  ... 
nl  n2  wei  latO  kk 

%  Allows  time  for  Condor  To  steady  out  during  live  testing 
if  isempty(kk) 
kk  =  0; 

end 

if  kk  <  1  &&  SIM==1 

X_HAT  =  zeros (16,1); 

else 

if  isempty(x  hat) 

%  Miscellaneous 
dt  =  0.01; 
g  =  9.8; 

[m/s''2  ] 
nl  =  7; 
n2  =  9; 

wei  =  7.292115*10^-5; 
lat0=  0.639268394832413;  %lon0 

%  AHRS  Initialization 
%  Euler  Angle  Initialization 

rx  =  ax  gps*cos (heading) +ay  gps*sin (heading)  ; 
ry  =  -ax_gps*sin (heading) +ay_gps*cos (heading) ; 
rz  =  az_gps-g; 

theta  0  =  atan((-rx*rz  -  fx*sqrt (rx^2+rz^2-fx^2) )  /  (fx^2-rz^2) ) ; 

r_theta  =  rx*sin (theta_0)  +  rz*cos (theta_0) ; 
fc  =  f y- speed* r; 


%Filter  dt  [sec] 
%Gravity  Constant 

%Number  of  AHRS  States 
%Number  of  INS  States 
%Sidereal  Rate  rad/s 
=  -2 . II5435878466264  [rad] 


%GPS  Position  (NED) 

%GPS  Heading 
%GPS  LTP  Down  Velocity 
%  (Only  used  in  Condor 
%  Simulation) 

%AHRS  Measurement  Noise 
%INS  Measurement  Noise 

%AHRS  Process  Noise 

%AHRS  Process  Noise 

%INS  Process  Noise 

%INS  Process  Noise 
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phi  0  =  atan((r  theta*ry  +  fc*sqrt (ry^2+r  theta^2-fc^2 ) )  /  (fc^2- 

r_theta''2 )  )  ; 
psi  0=heading; 

%  Euler  Angles  to  Quaternions 
q_0  =  e2q(phi_0,  theta_0,  psi_0); 

%  Gyro  Bias  Initial  Values 
bg_0  =  [0;  0;  0]  ; 

%  State  Vector 
x_hat  =  [q_0;  bg_0] ; 

%  Initial  Covariance  Estimate 
P  =  diag ( [ le-5*ones ( 1 , 4 )  le-3*ones ( 1 , 3 )  ]  )  ; 


%INS  Initialization 

%  Position  &  Velocity  Initialization 


II 

o 

1 

X 

N  sat; 

y_0  = 

E  sat; 

z  0  = 

D  sat; 

vn  0 

=  speed*cos 

(CoG) ; 

ve  0 

=  speed*sin 

(CoG) ; 

vd_0 

=  Vd  sat; 

%  Accelerometer  Initial  Bias  Estimate 
ba_0  =  [ 0 ;  0 ;  0 ]  ; 

%  State  Vector 

x_hat2  =  [x_0;  y_0;  z_0;  vn_0;  ve_0;  vd_0;  ba_0] ; 

%  Initial  Covariance  Estimate 

P2  =  diag ( [ le-5*ones ( 1 , 3 )  ,  le-5*ones ( 1 , 3 )  ,  le-3*ones ( 1 , 3 ) ] ) ; 
end 


%%  AHRS  ESTIMATOR 

%  Measurement  Process  Jacobian 
HHl  =  AHRS_H (x_hat) ; 

%  Kalman  Gain  Calculation 
K1  =  P*HH1' / (HH1*P*HH1'  +  R) ; 

%  Measurement  Processing 

rx  =  ax  gps*cos (heading) +ay  gps*sin (heading) ; 
ry  =  -ax_gps*sin (heading) +ay_gps*cos (heading) ; 
rz  =  az_gps-g; 

%  Theta 

theta  =  atan((-rx*rz  -  fx*sqrt  (rx''2+rz''2-fx^2)  )  /  (fx^2-rz''2)  )  ; 

r_theta  =  rx*sin (theta)  +  rz*cos (theta) ; 

%  Added  Coriolis  Term 

fc  =  fy-(norm(x  hat2 (4 : 6) ) ) *r; 

%  Phi 

phi  =  atan((r  theta*ry  +  fc*sqrt  (ry''2+r  theta''2-fc''2 )  )  /  (fc''2- 

r_theta^2 ) ) ; 

%  Heading  (Remove  [0  2pi]  restriction) 
if  isempty(Hl) 

HI  =  heading; 

H2  =  0; 

Headings  =  HI; 
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Heading  =  HI; 
ii=0 ; 

else 

H2  =  heading; 
if  (H2-H1)  <=  -100*pi/180 
ii  =  ii+1; 

end 

if  (H2-H1)  >=  100*pi/180 
ii  =  ii-1; 

end 

Heading  =  HeadingO+ ( (H2+2*pi*ii)  -  HeadingO) 
HI  =  H2; 

end 

%  Measurement  Vector 
Z  =  [phi;  theta;  Heading] ; 

%  Measurement  Estimate 

[phi_hat,  theta_hat,  psi_hat]  =  q2e(x_hat); 

%  Heading  (Remove  [0  2pi]  restriction) 
if  isempty(H3) 

H3  =  psi_hat; 

H4  =  0; 
psiO  =  H3; 
psi  =  H3; 
j  j=0; 

else 

H4  =  psi  hat; 

if  (H4-H3)  <=  -100*pi/180 

jj  =  jj+1; 

end 

if  (H4-H3)  >=  100*pi/180 

jj  =  jj-1; 

end 

psi  =  psi0+ ( (H4+2*pi* j j )  -  psiO); 

H3  =  H4; 

end 

%  Measurment  Estimate  Vecotr 
Z_hat  =  [phi_hat;  theta_hat;  psi] ; 

%  Measurement  Update 
X  hat  =  X  hat  +  Kl* (Z-Z  hat); 

%  Normalize  Quaternions 
Q  =  x_hat (1:4) ; 

Qn  =  Q/sqrt (Q' *Q) ; 
x_hat  =  [Qn;  x_hat(5:7)]; 

%  AHRS  State  Output  at  time  k 
X_AHRS  =  x_hat; 

%  Covariance  Update 
P=  (eye (nl, nl) -K1*HH1) *P; 

%  Time  Projection 
if  SIM  ==  1 


108 


F  =  AHRS  F  condor (x  hat,p,q,r); 

G  =  AHRS  G  condor (x  hat); 

Qv  =  Qv_C ; 

else 

F  =  AHRS_F_SEAFOX(x_hat,p,q,r,wei,latO)  ; 

G  =  AHRS_G_SEAFOX (x_hat)  ; 

Qv  =  Qv_S ; 

end 

%  Discretization  of  F  and  G 
OMEGA  =  [-F  G*Qv*G' ; 

zeros (nl , nl )  F' ] ; 

GAMMA  =  expm ( OMEGA* dt ) ; 

PHI  =  transpose (GAMMA ( (nl+l:2*nl) , (nl+1) :2*nl) ) ; 

Qd  =  PHI*GAMMA( (1 :nl) , (nl+1) :2*nl) ; 

%  Covariance  Time  Projection 
P  =  PHI*P*PHI'  +  Qd; 

%  State  Time  Projection 

w_bi  =  [p;  q;  r ] -x_hat ( 5 : 7 ) ; 
if  SIM  ==  1 

w  bt  =  w  bi; 

else 

R  t2b  =  rot  t2b(x  hat); 

w_bt  =  w_bi  -  R_t2b* [wei*cos (latO) ;  0;  -wei*sin (latO) ] ; 

end 


wl  =  w  bt(l);  w2  =  w  bt(2);  w3  =  w  bt(3); 

0  kpl  =  X  hat (1:4)  +  (dt/2)*[0  -wl  -w2  -w3; 

wl  0  w3  -w2; 
w2  -w3  0  wl; 


w3  w2  -wl  0]*x  hat  (1:4); 


%  Normalize  the  Quaternion 
Q  kpl  n  =  Q  kpl/sqrt(Q  kpl'*Q  kpl); 
x_hat  =  [Q_kpl_n;  x_hat(5:7)]; 


%%  INS  ESTIMATOR 


%  Measurement  Process  Jacobian 


HH2  =  [  1,  0,  0, 

0,  1,  0,  0, 
0,  0,  1,  0, 
0,  0,  0,  1, 
0,  0,  0,  0, 
0,  0,  0,  0, 


,  0 ,  0 ,  0 ,  0 ,  0  ; 
0 ,  0 ,  0 ,  0 ,  0  ; 

0 ,  0 ,  0 ,  0 ,  0 ; 

0 ,  0 ,  0 ,  0 ,  0  ; 

1,  0,  0,  0,  0; 

0 ,  1 ,  0 ,  0 ,  0  ]  ; 


%  Kalman  Gain  Calculation 
K2  =  P2*HH2' / (HH2*P2*HH2' +R2) ; 


%  Measurement 

Z2  =  [N_sat;  E_sat;  D_sat;  speed*cos (CoG) ;  speed*sin (CoG) ;  Vd_sat] 


Measurement  Estimate 
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Z2  hat  =  X  hat2(l:6); 

%  Measurement  State  Update 
X  hat2  =  X  hat2  +  K2*(Z2-Z2  hat); 

X~INS  =  x_hat2;  ~ 

%  Covariance  Update 

P2  =  (eye (n2, n2) -K2*HH2) *P2; 

if  SIM  ==  1 

F2  =  INS_F_condor (X_AHRS) ; 

G2  =  INS_G_condor (X_AHRS) ; 

Qv2  =  Qv2  C; 

else 

F2  =  INS_F_SEAFOX (X_AHRS,  wei,  latO); 

G2  =  INS_G_SEAFOX (X_AHRS) ; 

Qv2  =  Qv2  S ; 

end 

%  Discretization  of  F  and  G 
OMEGA2  =  [-F2  G2*Qv2*G2'; 

zeros (n2,n2)  F2']; 

GAMMA2  =  expm(0MEGA2*dt) ; 

PHI2  =  transpose (GAMMA2 ( (n2+l : 2 *n2 ), (n2+l ): 2 *n2 )) ; 

Qd2  =  PHI2*GAMMA2 ( (1 :n2) , (n2+l) :2*n2) ; 

%  Covariance  Time  Projection 
P2  =  PHI2*P2*PHI2'  +  Qd2; 

%  State  Time  Projection 
R_t2b  =  rot_t2b (X_AHRS) ; 

R  b2t  =  R  t2b' ; 

P  kpl  =  X  hat2(l:3)  +  [x  hat2(4:5);  -x  hat2 (6) ] *dt; 

V  kpl  =  X  hat2(4:6)  +  (R  b2t*([fx;  fy;  fz]  -  x  hat2(7:9))  +  [0;  0 
g] ) *dt; 

X  hat2  =  [P  kpl;  V  kpl;  x  hat2(7:9)]; 

X_HAT  =  [X_AHRS (1 : 4) ;  X_INS(1:6);  X_AHRS(5:7);  X_INS(7:9)]; 
end 

kk  =  kk+1; 

function  R  t2b  =  rot  t2b(x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

R_t2b  =  [q0^2+ql^2-q2^2-q3^2  2* (ql*q2+q0*q3)  2*(ql*q3-q0*q2); 

2* (q2*ql-q0*q3)  qO ^2 -ql ^2+q2 ^2 -q3 ^2  2*(q2*q3+q0*ql); 

2*  (q3*ql  +  q0*q2)  2  *  ( q3  *q2 -qO  *ql )  qO  ^2 -ql '^2 -q2  ^2  +  q3 '^2  ]  ; 


end 

function  G  =  INS  G  condor (x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

no 


G  = 

[0,  0, 

0; 

0,  0, 

0; 

0,  0, 

0; 

-  qO- 

( — 1 

1 

csi 

'2  +  q2^2 

+  q3' 

'2, 

2*ql 

*q3; 

-  2*q0*q3  - 

2*ql*q2, 

-  qO' 

'2  + 

2*q2 

*q3; 

2*q0* 

q2  -  2’ 

‘'ql*q3,  - 

2*q0' 

^ql 

q3^2 

r 

0,  0, 

0; 

0,  0, 

0; 

0,  0, 

0]  ; 

2*q0*q3  -  2*ql*q2,  -  2*q0*q2  - 
ql^2  -  q2^2  +  q3^2,  2*q0*ql  - 
-  2*q2*q3,  -  q0''2  +  ql''2  +  q2^2  - 


end 


function  G  =  INS_G_SEAFOX (x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 


G  = 

[0, 

0, 

0, 

0, 

0, 

0; 

0, 

0, 

0, 

0, 

0, 

0; 

0, 

0, 

0, 

0, 

0, 

0; 

-qO 

^2 

-  ql 

^2 

+ 

q2  ^2  + 

q3 

"2, 

2*q0*q3  -  2* 

ql*q2. 

-  2*q0*q2 

2* 

ql* 

q3. 

0, 

0, 

0; 

-2* 

qO* 

q3  - 

2 

*ql 

*q2,  - 

qO 

^2  + 

ql^2  -  q2^2 

+  q3^2 

,  2*q0*ql 

2* 

q2* 

q3. 

0, 

0, 

0; 

2* 

qO* 

q2  - 

2 

*ql 

*q3,  - 

2* 

q0*ql  -  2*q2*q3, 

-  q0^2 

+  ql^2  + 

q3 

"2, 

0, 

0, 

0; 

0, 

0, 

0, 

1, 

0, 

0; 

0, 

0, 

0, 

0, 

1, 

0; 

0, 

0, 

0, 

0, 

0, 

1]  ; 

end 

function  F 

=  INS 

F 

condor (x) 

qO  = 

X  ( 1 )  ; 

ql 

= 

X  ( 2  )  ;  q2 

=  X 

(3)  ; 

q3  =  X  ( 4 )  ; 

F  = 

[0, 

0, 

0, 

1, 

0, 

0,  0, 

0, 

0; 

0, 

0, 

0, 

0, 

1, 

0,  0, 

0, 

0; 

0, 

0, 

0, 

0, 

0, 

-1,  0 

,  0 

,  0; 

0, 

0, 

0, 

0, 

0, 

0,  - 

< 

o 

2  - 

ql^2  +  q2^2 

+  q3^2. 

2*q0*q3 

- 

2*q0*q2 

- 

2* 

ql*q3; 

0, 

0, 

0, 

0, 

0, 

0,  - 

2*q0*q3 

-  2*ql*q2, 

-  q0"2 

+  ql^2  - 

q2 

^2 

+  q3 

/'2 

,  2 

*q0*ql 

- 

CM 

CM 

*q3; 

0, 

0, 

0, 

0, 

0, 

0,  2* 

qO* 

q2  - 

2*ql*q3,  .. 

- 

2*q0*ql 

- 

2* 

q2*q3. 

- 

q0^2 

+  ql^2  +  q2 

"2  -  q3 

"2; 

0, 

0, 

0, 

0, 

0, 

0,  0, 

0, 

0; 

0, 

0, 

0, 

0, 

0, 

0,  0, 

0, 

0; 

0, 

0, 

0, 

0, 

0, 

0,  0, 

0, 

0]  ; 

end 

function  F 

=  INS 

F 

SEAFOX (x. 

wei 

,  latO) 

qO  = 

X  ( 1 )  ; 

ql 

= 

X  ( 2  )  ;  q2 

=  X 

(3)  ; 

q3  =  X  ( 4 )  ; 

F  = 

[0, 

0, 

0, 

1, 

0, 

0,  0, 

0, 

0; 

0, 

0, 

0, 

0, 

1, 

0,  0, 

0, 

0; 

0, 

0, 

0, 

0, 

0, 

-1,  0 

,  0 

,  0; 

-  2*ql*q2, 
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0,  0,  0,  0,  -2*wei*sin (latO) ,  0,  -  q0^2  -  ql^2  +  q2^2  +  q3^2,  ... 

2*q0*q3  -  2*ql*q2,-  2*q0*q2  -  2*ql*q3; 

0,  0,  0,  2*wei*sin (latO)  ,  0,  2*wei*cos (latO) ,  -  2*q0*q3  -  2*ql*q2, 

-  q0^2  +  ql-'2  -  q2^2  +  q3''2,  2*q0*ql  -  2*q2*q3; 

0,  0,  0,  0,  -2*wei*cos (latO) ,  0,  2*q0*q2  -  2*ql*q3,  -  2*q0*ql  ... 

-  2*q2*q3,  -  q0-'2  +  ql^2  +  q2''2  -  q3^2; 


0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0; 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0; 

0, 

end 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0]  ; 

function  G  = 

AHRS  G 

SEAFOX (x) 

qO  = 

X (1)  ;  ql 

=  x(2) 

;  q2  = 

x(3)  ; 

q3 

=  x(4)  ; 

G  = 

[  ql/2. 

q2/2. 

q3/2. 

0, 

0, 

0; 

-qO/2, 

q3/2. 

-q2/2. 

0, 

0, 

0; 

-q3/2. 

-qO/2, 

ql/2. 

0, 

0, 

0; 

q2/2. 

-ql/2. 

-qO/2, 

0, 

0, 

0; 

0, 

0, 

0, 

1, 

0, 

0; 

0, 

0, 

0, 

0, 

1, 

0; 

0, 

0, 

0, 

0, 

0, 

1]  ; 

end 

function  G  = 

AHRS  G 

condor (x) 

qO  = 

X (1)  ;  ql 

=  x(2) 

;  q2  = 

X  ( 3 )  ; 

q3 

=  x(4)  ; 

G  = 

[  ql/2. 

q2/2. 

q3/2; 

-qO/2, 

q3/2. 

-q2/2; 

-q3/2. 

-qO/2, 

ql/2; 

q2/2. 

-ql/2. 

-qO/2; 

0, 

0, 

0; 

0, 

0, 

0; 

0, 

0, 

0] 

r 

end 

function  F  = 

AHRS  F 

condor (x. 

p. 

q. 

r) 

%  Quaternions 

qO  = 

X (1)  ;  ql 

=  x(2) 

;  q2  = 

X  ( 3 )  ; 

q3 

=  x(4)  ; 

%  Gyro  Bias 

bgl  =  x(5);  bg2  =  x(6);  bg3  =  x(7); 


F  =  [0,  bgl/2  -  p/2,  bg2/2  - 


p/2 

-  bgl/2. 

0, 

q2/2; 

q/2 

-  bg2/2. 

bg3/2 

-  r/2 

ql/2; 

r/2 

-  bg3/2. 

q/2  - 

bg2  /2 

qO/2; 

0, 

0 

0; 

q/2,  bg3/2  -  r/2, 
r/2  -  bg3/2,  bg2/2 

,  0,  p/2  - 

,  bgl/2  -  p/2, 

,  0, 


ql/2, 
-  q/2 

bgl/2 

0 


q2/2, 

-qO/2, 

-q3/2, 

q2/2, 

0,  0, 


q3/2; 
q3/2,  - 

-qO/2, 

-ql/2,  - 


0, 
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0; 

0, 

0, 

0, 

0, 

0, 

0, 

0]  ; 
end 

0, 

0, 

0, 

0, 

0, 

0, 

function  F  =  AHRS  F  SEAFOX(x,  p,  q,  r,  wei,  latO) 

%  Quaternions 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

%  Gyro  Bias 

bgl  =  x(5);  bg2  =  x(6);  bg3  =  x(7); 

F  =  [qO*wei* (ql*cos (latO)  -  q3*sin (latO) ) ,  ... 

(wei*cos (latO) *q0^2) /2  +  ( 3*wei*cos ( latO ) *ql ^2 ) /2  -  ... 
wei*sin (latO) *ql*q3  +  (wei*cos (latO) *q2^2) /2  +  ... 

(wei*cos  (latO)  *q3''2) /2  +  bgl/2  -  p/2,  ... 

bg2/2  -  q/2  +  ql*q2*wei*cos (latO)  -  q2*q3*wei*sin (latO) ,  ... 

bg3/2  -  r/2  -  (q0''2*wei*sin  (latO)  ) /2  -  (ql''2*wei*sin  (latO)  ) /2  -  ... 
(q2''2*wei*sin  (latO)  ) /2  -  (3*q3''2*wei*sin  (latO)  ) /2  + 
ql *q3*wei*cos (latO) ,  ... 

ql/2,  q2/2,  q3/2; 

p/2  -  bgl/2  -  (3*q0^2*wei*cos (latO) ) /2  -  (ql^2*wei*cos (latO) ) /2  -  ... 
(q2''2*wei*cos  (latO)  ) /2  -  (q3''2*wei*cos  (latO)  ) /2  -  q0*q2*wei*sin  (latO)  , 

-ql*wei* (qO*cos (latO)  +  q2*sin (latO) ) ,  r/2  -  bg3/2  -  ... 

(q0''2*wei*sin  (latO)  ) /2  -  (ql''2*wei*sin  (latO)  ) /2  -  ... 

(3*q2^2*wei*sin (latO) ) /2  -  (q3^2*wei*sin (latO) ) /2  -  ... 
q0*q2*wei*cos (latO)  ,  bg2/2  -  q/2  -  q0*q3*wei*cos (latO)  -  ... 
q2*q3*wei*sin (latO) ,  -qO/2,  q3/2,  -q2/2; 

q/2  -  bg2/2  +  q0*q3*wei*cos (latO)  +  qO*ql*wei*sin (latO) ,  ... 

(wei*sin  (latO)  *q0''2) /2  +  (3*wei*sin  (latO)  *ql''2) /2  +  ... 
wei*cos ( latO ) *ql *q3  +  (wei*sin (latO) *q2^2) /2  +  ... 

(wei*sin  (latO)  *q3''2) /2  +  bg3/2  -  r/2,  ... 

q2*wei* (q3*cos (latO)  +  ql*sin (latO) ) ,  ... 

(wei*cos (latO) *q0^2) /2  +  (wei*cos ( latO ) *ql ^2 ) /2  +  wei*sin (latO) *ql*q3  + 

(wei*cos (latO) *q2^2) /2  +  (3*wei*cos (latO) *q3^2) /2  -  bgl/2  +  ... 
p/2,  -q3/2,  -qO/2,  ql/2; 

(3*wei*sin  (latO)  *q0''2) /2  -  wei*cos  (latO)  *q0*q2  +  ... 

(wei*sin (latO) *ql^2) /2  +  (wei*sin (latO) *q2^2) /2  +  ... 

(wei*sin  (latO)  *q3''2) /2  -  bg3/2  +  r/2,  q/2  -  ... 
bg2/2  -  ql*q2*wei*cos (latO)  +  qO*ql*wei*sin (latO) ,  ... 

bgl/2  -  p/2  -  (q0''2*wei*cos  (latO)  ) /2  -  (ql''2*wei*cos  (latO)  ) /2  -  ... 
(3*q2^2*wei*cos (latO) ) /2  -  (q3^2*wei*cos (latO) ) /2  +  ... 
q0*q2*wei*sin (latO) ,  -q3*wei* (q2*cos (latO)  -  qO*sin (latO) ) ,  ... 


q2/2. 

-ql/2, 

■qO/2 

r 

0, 

0, 

0,  0, 

0, 

0, 

0; 

0, 

0, 

0,  0, 

0, 

0, 

0; 

0, 

0, 

0,  0, 

0, 

0, 

0]  ; 

end 

function  H  =  AHRS  H(x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

H  =  [  -(2*(q0^2*ql  +  2*q0*q2*q3  +  ql-'O  +  ql*q2''2  -  ql*q3-'2))/  ... 
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(((4*(q0*ql  +  q2*q3) "2) / (q0^2  -  ql^2  -  q2^2  +  q3^2)^2  +  1)*  ... 

(q0^2  -  ql''2  -  q2^2  +  q3''2)^2),  (2*(q0^3  +  q0*ql^2  -  q0*q2^2  +  .. 

q0*q3^2  +  2*ql*q2*q3)  )  /  (  (  (4*  (qO*ql  +  q2  *q3 )  ^2  )  /  ( qO ''2  -  ql^2  -  ... 
q2^2  +  q3^2)^2  +  l)*(q0^2  -  ql^2  -  q2^2  +  q3^2)^2),  (2*(q0^2*q3  +  .. 

2*q0*ql*q2  -  ql^2*q3  +  q2^2*q3  +  q3  ^  3 )  )  /  (  (  ( 4  *  ( qO  *ql  +  q2*q3)'"2)/  ... 
(q0^2  -  ql^2  -  q2^2  +  q3^2)^2  +  l)*(q0^2  -  ql^2  -  q2^2  +  q3^2)^2),  . 

-(2*(-  q0^2*q2  +  2*q0*ql*q3  +  ql-'2*q2  +  q2''3  +  q2  *q3 '^2 ))/((( 4  *  ... 
(qO*ql  +  q2*q3)  ^2)  /  (q0''2  -  ql^2  -  q2''2  +  q3^2)''2  +  l)*(q0^2  -  ql''2  . 

-  q2"2  +  q3^2)  "2)  ,  0,  0,  0; 

(2*q2)/(l  -  (2*q0*q2  -  2  *ql  *q3 )  "2  )  ■"  ( 1 /2  )  ,  -(2*q3)/(l  -  (2*q0*q2  -  .. 
2*ql*q3) "2) ^ (1/2) ,  (2*q0)/(l  -  (2*q0*q2  -  2 *ql *q3 ) "2 ) ^ ( 1 /2 ) ,  ... 

-(2*ql)/(l  -  (2*q0*q2  -  2  *ql  *q3 ) '"2  )  "  ( 1 /2  )  ,  0,  0,  0; 

-(2*(q0^2*q3  +  2*q0*ql*q2  -  ql''2*q3  +  q2-"2*q3  +  q3 3 )  )  /  (  (  ( 4 *  (qO *q3  . 

+  ql*q2) ^2) / (q0^2  +  ql^2  -  q2^2  -  q3^2)^2  +  l)*(q0^2  +  ql^2  -  q2^2  . 

-  q3''2)^2),  -(2*(-  q0^2*q2  +  2*q0*ql*q3  +  ql-'2*q2  +  q2-'3  +  ... 

q2*q3^2)  ) / (  (  (4* (q0*q3  +  ql*q2)  "2)  / (q0^2  +  ql"2  -  q2^2  -  q3"2)^2  +  .. 
I)*(q0''2  +  ql^2  -  q2''2  -  q3^2)''2),  (2*(q0^2*ql  +  2*q0*q2*q3  +  ql''3  + 

ql*q2^2  -  ql *q3 ^2 )  ) / (  (  ( 4 * ( qO *q3  +  ql *q2  )  "2  )  / ( qO ^2  +  ql"2  -  q2^2  -  .. 
q3-'2)^2  +  l)*(q0''2  +  ql^2  -  q2''2  -  q3^2)''2),  (2*(q0''3  +  q0*ql-'2  . 

-  q0*q2^2  +  q0*q3^2  +  2 *ql *q2 *q3 )  ) / (  (  ( 4 * ( q0*q3  +  ql*q2 ) ^2  )  / (q0^2  +  . 
ql''2  -  q2^2  -  q3''2)"'2  +  l)*(q0''2  +  ql''2  -  q2''2  -  q3''2)''2),  0,  0,  0]; 
end 


function  q  =  e2q(phi,  theta,  psi) 

%  Form  Rotation  Matrix 

R_psi  =  [cos (psi)  sin (psi)  0;  -sin (psi)  cos (psi)  0;  0  0  1]; 

R_theta  =  [cos (theta)  0  -sin (theta)  ;  0  1  0;  sin (theta)  0  cos  (theta) ] 
R_phi  =[10  0;  0  cos (phi)  sin (phi);  0  -sin (phi)  cos  (phi) ]; 

R  n2b  =  R  phi*R  theta*R  psi; 

%  Extract  Quaternions 
q0=sqrt (1+trace (R_n2b) ) /2; 
ql= (R_n2b (2,3) -R_n2b (3,2) ) / (4*q0) ; 
q2= (R_n2b (3, 1) -R_n2b (1, 3) ) / (4*q0)  ; 
q3= (R_n2b (1,2) -R_n2b (2, 1)  )  / (4*q0)  ; 

q  =  [qO;  ql;  q2 ;  q3]; 
end 


function  [phi,  theta,  psi]  =  q2e (x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

%  Compute  Euler  Angles 

phi  =  atan2 (2* (q2*q3+q0*ql )  ,  (q0^2-ql^2-q2^2+q3^2)  )  ; 
theta  =  asin (-2* (ql*q3-q0*q2) ) ; 

psi  =  mod (atan2 (2* (ql*q2+q0*q3) , (q0^2+ql^2-q2^2-q3^2) ) , 2*pi) ; 
end 


end 


2,  Measurement  Model  Two  Implementation 


function  X  HAT  =  EKF2 (u) 
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%%  Extended  Kalman  Filter  for  Navigation  State  Estimation 
%  Measurement  Model  2 
%  LT  Steven  Terjesen 
%  September  2014 

%  **  This  EKE  was  unable  to  converge  when  running  measurement  model  2 
*  * 


%  This  estimator  is  built  for  use  in  MATLAB  Simulink.  There  are  39 
inputs 

%  required  and  can  be  run  in  Simulink  with  an  "Interpolated  MATLAB 
%  Function"  block.  The  first  13  inputs  are  the  vehicle  data:  Course 
Over 

%  Ground,  Speed  Over  Ground,  Accelerometer  Measurements,  Gyro 
Measurements , 

%  GPS  Measurements  (In  LTP  XNorth-YEast-ZDown) ,  Heading  Measurement, 
and 

%  Vertical  Velocity  (zeroed  out  for  SEAFOXII  data) .  Inputs  14  through 
35 

%  are  the  Process  Noise  and  Measurement  Noise  diagonal  elements.  These 
%  elements  are  not  hard  coded  to  allow  for  easier  tuning.  Inputs  36 
%  through  38  are  the  N-E-D  accelerations  in  the  LTP  frame.  Input  39  is 
a 

%  toggle  for  selecting  whether  the  data  is  from  Condor  or  SEAFOX  II. 


%%  System  Inputs 
%  Measurement  inputs 
CoG  =  u ( 1 ) ; 
speed  =  u (2 ) ; 

fx  =  u(3);  fy  =  u(4);  fz  =  u(5); 
p  =  u(6);  q  =  u(7);  r  =  u(8); 

N_sat  =  u(9);  E_sat  =  u(10);  D_sat  =  u(ll); 
heading  =  u ( 12 ) ; 

Vd  sat  =  u  (13) ; 


%  Process  and  Measurement  Noise  Matrices 
R  =  diag (u  ( 14  : 17 ) )  ; 

R2  =  diag (u ( 1 8 : 23 ) )  ; 


%GPS  Course  Over  Ground 
%GPS  Speed  Over  Ground 
%IMU  Accelerations 
%IMU  Angular  Rates 
%GPS  Position  (NED) 

%GPS  Heading 
%GPS  LTP  Down  Velocity 
%  (Only  used  in  Condor 
%  Simulation) 

%AHRS  Measurement  Noise 
%INS  Measurement  Noise 


Qv_C  =  diag (u (24 : 26) ) ; 
Condor 

Qv_S  =  diag (u (24 : 29) ) ; 
SEAFOX 

Qv2_C  =  diag (u ( 30 : 32 ) ) ; 
Condor 

Qv2_S  =  diag (u  (30 : 35) )  ; 
SEAFOX 


%AHRS  Process  Noise 
%AHRS  Process  Noise 
%INS  Process  Noise 
%INS  Process  Noise 


%  GPS  Accelerations 

ax_gps  =  u(36);  ay_gps  =  u(37);  az_gps  =  u(38);  %GPS  Accelerations  as 

%  calculated  from  3rd 
%  Order  Filter 
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Condor (1)  SEAF0X(2) 


SIM  =  u (39) ; 

%%  Initialization 

persistent  x  hat  x  hat2  P  P2  g  dt  ii  jj  HI  H2  H3  H4  HeadingO  psiO  .. . 
nl  n2  wei  latO  kk 

%  Allows  time  for  Condor  To  steady  out  during  live  testing 
if  isempty(kk) 
kk  =  0; 

end 

if  kk  <  1  &&  SIM==1 

X  HAT  =  zeros (16, 1) ; 

else 

if  isempty(x  hat) 

%  Miscellaneous 
dt  =  0.01; 
g  =  9.8; 

[m/s’'2] 
nl  =  7; 
n2  =  9; 

wei  =  7.292115*10^-5; 
lat0=  0.639268394832413;  %lon0 

%  AHRS  Initialization 
%  Euler  Angle  Initialization 

rx  =  ax  gps*cos (heading) +ay  gps*sin (heading)  ; 
ry  =  -ax_gps*sin (heading) +ay_gps*cos (heading) ; 
rz  =  az_gps-g; 

theta  0  =  atan((-rx*rz  -  fx*sqrt (rx^2+rz^2-fx^2 ) )  /  (fx^2-rz^2) ) ; 

r_theta  =  rx*sin (theta_0)  +  rz*cos (theta_0) ; 
fc  =  f y- speed* r; 

phi  0  =  atan((r  theta*ry  +  fc*sqrt (ry^2+r  theta^2-fc^2 ) )  /  (fc^2- 

r_theta''2 )  )  ; 
psi  0=heading; 

%  Euler  Angles  to  Quaternions 
q_0  =  e2q(phi_0,  theta_0,  psi_0); 

%  Gyro  Bias  Initial  Values 
bg_0  =  [0;  0;  0]  ; 

%  State  Vector 
x_hat  =  [q_0;  bg_0] ; 

%  Initial  Covariance  Estimate 
P  =  diag ( [ le-3*ones ( 1 , 4 )  le-3*ones (1, 3) ] ) ; 

%INS  Initialization 

%  Position  &  Velocity  Initialization 
x_0  =  N_sat; 
y_0  =  E_sat; 
z_0  =  D_sat; 
vn_0  =  speed*cos (CoG) ; 
ve_0  =  speed*sin (CoG) ; 
vd_0  =  Vd_sat; 

%  Accelerometer  Initial  Bias  Estimate 
ba_0  =  [ 0 ;  0 ;  0 ]  ; 

%  State  Vector 

x_hat2  =  [x_0;  y_0;  z_0;  vn_0;  ve_0;  vd_0;  ba_0] ; 
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%Filter  dt  [sec] 
%Gravity  Constant 

%Number  of  AHRS  States 
%Number  of  INS  States 
%Sidereal  Rate  rad/s 
=  -2.115435878466264  [rad] 


%  Initial  Covariance  Estimate 

P2  =  diag ( [le-3*ones (1, 3) ,  le-3*ones (1, 3) ,  le-3*ones (1, 3) ] ) ; 
end 


%%  AHRS  ESTIMATOR 
%  Measurement  Process  Jacobian 

H  =  AHRS_H (x_hat,  x_hat2,  p,  q,  r,  g,  ax_gps,  ay_gps,  az_gps) 

%  Kalman  Gain  Calculation 
K  =  P*H' / (H*P*H'  +  R)  ; 

%  Measurement  Processing 
%  Heading  (Remove  [0  2pi]  restriction) 
if  isempty(Hl) 

HI  =  heading; 

H2  =  0; 

Headings  =  HI; 

Heading  =  HI; 
ii=0 ; 

else 

H2  =  heading; 
if  (H2-H1)  <=  -100*pi/180 
ii  =  ii+1; 

end 

if  (H2-H1)  >=  100*pi/180 
ii  =  ii-1; 

end 

Heading  =  Heading0+ ( (H2+2*pi*ii)  -  HeadingO); 

HI  =  H2; 

end 

%  Measurement  Vector 
Z  =  [fx;  fy;  fz;  Heading]; 

%  Measurement  Estimate 

[~,  psi_hat]  =  q2e(x_hat); 

%  Heading  (Remove  [0  2pi]  restriction) 
if  isempty(H3) 

H3  =  psi_hat; 

H4  =  0; 
psiO  =  H3; 
psi  =  H3; 
j  j=0; 

else 

H4  =  psi  hat; 

if  (H4-H3)  <=  -100*pi/180 

jj  =  jj+1; 

end 

if  (H4-H3)  >=  100*pi/180 

jj  =  jj-1; 

end 

psi  =  psi0+ ( (H4+2*pi* j j )  -  psiO); 

H3  =  H4; 

end 


117 


R  t2b  =  rot  t2b(x  hat); 

w_bi  =  [p;  q;  r]  -  x_hat(5:7); 

if  SIM  ==  1 

w  bt  =  w  bi; 

else 

w_bt  =  w_bi  -  R_t2b* [wei*cos (latO) ;  0;  -wei*sin (latO) ] 

end 

Fb_hat  =  R_t2b* [ax_gps;  ay_gps;  az_gps]  +  cross (w_bt, 

R  t2b*x  hat2(4:6))  +  x  hat2(7:9); 

%  Measurment  Estimate  Vecotr 
Z_hat  =  [Fb_hat;  psi]  ; 

%  Measurement  Update 
X  hat  =  X  hat  +  K* (Z-Z  hat)  ; 

%  Normalize  Quaternions 
Q  =  x_hat (1:4) ; 

Qn  =  Q/sqrt (Q' *Q) ; 
x_hat  =  [Qn;  x_hat(5:7)]; 

%  AHRS  State  Output  at  time  k 
X_AHRS  =  x_hat; 

%  Covariance  Update 
P  =  (eye (nl, nl) -K*H) *P; 

%  Time  Projection 
if  SIM  ==  1 

F  =  AHRS  F  condor (x  hat,p,q,r); 

G  =  AHRS  G  condor (x  hat); 

Qv  =  Qv_C ; 

else 

F  =  AHRS_F_SEAFOX(x_hat,p,q,r,wei,latO)  ; 

G  =  AHRS_G_SEAFOX (x_hat)  ; 

Qv  =  Qv_S; 

end 

%  Discretization  of  F  and  G 
OMEGA  =  [-F  G*Qv*G' ; 

zeros(nl,nl)  F']; 

GAMMA  =  expm ( OMEGA* dt ) ; 

PHI  =  transpose (GAMMA ( (nl  +  l:2*nl)  ,  (nl  +  1)  :2*nl) ) ; 

Qd  =  PHI*GAMMA( (1 :nl) , (nl+1) :2*nl) ; 

%  Covariance  Time  Projection 
P  =  PHI*P*PHI'  +  Qd; 

%  State  Time  Projection 

w_bi  =  [p;  q;  r ] -x_hat ( 5 : 7 ) ; 
if  SIM  ==  1 

w  bt  =  w  bi; 
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else 


R  t2b  =  rot  t2b(x  hat); 

w_bt  =  w_bi  -  R_t2b* [wei*cos (latO) ;  0;  -wei*sin (latO) ] ; 

end 


wl  =  w  bt(l);  w2  =  w  bt(2);  w3  =  w  bt(3); 

Q  kpl  =  X  hat (1:4)  +  (dt/2)*[0  -wl  -w2  -w3; 

wl  0  w3  -w2; 
w2  -w3  0  wl; 


w3  w2  -wl  0]*x  hat  (1:4); 


%  Normalize  the  Quaternion 
Q  kpl  n  =  Q  kpl/sqrt(Q  kpl'*Q  kpl); 
x_hat  =  [Q_kpl_n;  x_hat(5:7)]; 


%%  INS  ESTIMATOR 


%  Measurement  Process  Jacobian 


H 


1,  0,  0,  0,  0, 

0,  1,  0,  0,  0, 

0,  0,  1,  0,  0, 

0,  0,  0,  1,  0, 

0,  0,  0,  0,  1, 

0 ,  0 ,  0 ,  0 ,  0 , 


0,  0, 

0,  0, 

0,  0, 

0,  0, 

0,  0, 

1,  0, 


0, 

0, 

0, 

0, 

0, 

0, 


0; 
0; 
0; 
0; 
0; 
0]  ; 


%  Kalman  Gain  Calculation 
K  =  P2*H' / (H*P2*H' +R2) ; 


%  Measurement 

Z  =  [N_sat;  E_sat;  D_sat;  speed*cos (CoG) ;  speed*sin (CoG)  ;  Vd_sat] 

%  Measurement  Estimate 
Z_hat  =  x_hat2(l:6); 

%  Measurement  State  Update 
X  hat2  =  X  hat2  +  K*(Z-Z  hat); 

X~INS  =  x_hat2;  ~ 

%  Covariance  Update 

P2  =  (eye  (n2,  n2) -K*H)  *P2; 

if  SIM  ==  1 

F  =  INS_F_condor (X_AHRS) ; 

G  =  INS_G_condor (X_AHRS) ; 

Qv2  =  Qv2  C; 

else 

F  =  INS_F_SEAFOX (X_AHRS,  wei,  latO); 

G  =  INS_G_SEAFOX (X_AHRS) ; 

Qv2  =  Qv2_S; 

end 


%  Discretization  of  F  and  G 
OMEGA  =  [-F  G*Qv2*G'; 

zeros (n2,n2)  F' ] ; 
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GAMMA  =  expm ( OMEGA* dt ) ; 

PHI  =  transpose (GAMMA ( (n2  +  l : 2  *n2 ) ,  (n2  +  l )  : 2  *n2 ) ) ; 

Qd  =  PHI*GAMMA( ( 1 : n2 ) , (n2+l) :2*n2) ; 

%  Covariance  Time  Projection 
P2  =  PHI*P2*PHI'  +  Qd; 

%  State  Time  Projection 
R_t2b  =  rot_t2b (X_AHRS) ; 

R  b2t  =  R  t2b' ; 

P  kpl  =  X  hat2(l:3)  +  [x  hat2(4:5);  -x  hat(6)]*dt; 

V_k;pl  =  x_hat2(4:6)  +  (R_b2t*  (  [ fx;  fy;  fz]  -  x_hat2(7:9))  +  [0;  0; 
g] ) *dt; 

X  hat2  =  [P  kpl;  V  kpl;  x  hat2(7:9)]; 

X_HAT  =  [X_AHRS;  X_INS]; 
end 

kk  =  kk+1; 

function  R  t2b  =  rot  t2b(x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

R_t2b  =  [q0^2+ql^2-q2^2-q3^2  2* (ql*q2+q0*q3)  2*(ql*q3-q0*q2); 

2*  (q2*ql-q0*q3)  qO ^2 -ql ''2+q2 ^2 -q3 '^2  2*(q2*q3+q0*ql); 

2* (q3*ql  +  q0*q2)  2  * ( q3 *q2 -qO *ql )  qO ^2 -ql ^2 -q2 ^2  +  q3 ^2 ] ; 


end 


function  G 

= 

INS 

G 

condor (x) 

qO  = 

X  ( 1 )  ; 

ql 

= 

X  ( 2  )  ; 

q2  = 

=  X 

G  = 

[0, 

0, 

0; 

0, 

0, 

0; 

0, 

0, 

0; 

- 

< 

O 

2  - 

ql 

^2 

+  q2  ^2 

+  ( 

( — 1 

CM 

*q3 

r 

- 

2*q0*q3  - 

2* 

ql* 

q2. 

—  ( 

2*q2 

*q3 

r 

2* 

qO* 

q2 

-  2 

*ql 

*q3 

r  ~ 

2*< 

q3^2 

r 

0, 

0, 

0; 

0, 

0, 

0; 

0, 

0, 

0] 

r 

end 

function  G 

= 

INS 

G 

SEAFOX (x) 

qO  = 

X  ( 1 )  ; 

ql 

= 

X  ( 2  )  ; 

q2  = 

=  X 

G  = 

[0, 

0, 

0, 

0, 

0, 

0; 

0, 

0, 

0, 

0, 

0, 

0; 

0, 

0, 

0, 

0, 

0, 

0; 

-qO 

^2 

-  ql ^2 

+ 

< 

CM 

2  + 

q3' 

2* 

ql* 

q3. 

0, 

0, 

0; 

-  2*ql*q2,  -  2*q0*q2 
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2* 

qO* 

q3  - 

-  2^ 

''ql  ■ 

"q2. 

2* 

q2* 

q3. 

0, 

0, 

0; 

2* 

qO* 

q2  - 

-  2^ 

''ql' 

"q3. 

q3 

-2, 

0, 

0, 

0; 

0, 

0, 

0, 

1, 

0, 

0; 

0, 

0, 

0, 

0, 

1, 

0; 

0, 

0, 

0, 

0, 

0, 

1]  ; 

end 


q0^2  +  ql^2  -  q2^2  +  q3^2,  2*q0*ql  -  ... 
2*q0*ql  -  2*q2*q3,  -  q0^2  +  ql-'2  +  q2^2  -  .  .  . 


function  F  =  INS  F  condor (x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

F  =  [0,  0,  0,  1,  0,  0,  0,  0,  0; 

0,  0,  0,  0,  1,  0,  0,  0,  0; 

0,  0,  0,  0,  0,  -1,  0,  0,  0; 

0,  0,  0,  0,  0,  0,  -  q0^2  -  ql^2  +  q2^2  +  q3^2,  2*q0*q3  -  2*ql*q2, 

-  2*q0*q2  -  2*ql*q3; 

0,  0,  0,  0,  0,  0,  -  2*q0*q3  -  2*ql*q2,  -  q0^2  +  ql^2  -  ... 

q2^2  +  q3''2,  2*q0*ql  -  2*q2*q3; 

0,  0,  0,  0,  0,  0,  2*q0*q2  -  2*ql*q3,  ... 

-  2*q0*ql  -  2*q2*q3,  -  q0^2  +  ql^2  +  q2^2  -  q3^2; 

0,  0,  0,  0,  0,  0,  0,  0,  0; 

0,  0,  0,  0,  0,  0,  0,  0,  0; 

0,  0,  0,  0,  0,  0,  0,  0,  0]; 

end 

function  F  =  INS_F_SEAFOX (x,  wei,  latO) 
qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

F  =  [0,  0,  0,  1,  0,  0,  0,  0,  0; 

0,  0,  0,  0,  1,  0,  0,  0,  0; 

0,  0,  0,  0,  0,  -1,  0,  0,  0; 

0,  0,  0,  0,  -2*wei*sin (latO) ,  0,  -  q0^2  -  ql^2  +  q2^2  +  q3^2,  ... 

2*q0*q3  -  2*ql*q2,-  2*q0*q2  -  2*ql*q3; 

0,  0,  0,  2*wei*sin (latO) ,  0,  2*wei*cos (latO) ,  -  2*q0*q3  -  2*ql*q2, 

-  q0^2  +  ql-'2  -  q2^2  +  q3''2,  2*q0*ql  -  2*q2*q3; 

0,  0,  0,  0,  -2*wei*cos (latO) ,  0,  2*q0*q2  -  2*ql*q3,  -  2*q0*ql  ... 

-  2*q2*q3,  -  q0-'2  +  ql^2  +  q2-'2  -  q3^2; 


0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0; 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0; 

0, 

end 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0]  ; 

function  G  = 

AHRS  G 

SEAFOX (x) 

qO  =  X  (1)  ;  ql 

=  x(2) 

;  q2  = 

X  ( 3 )  ; 

q3  =  X  ( 4 )  ; 

G  =  [  ql/2. 

q2/2. 

q3/2. 

0, 

0, 

0; 

-qO/2, 

q3/2. 

-q2/2. 

0, 

0, 

0; 

-q3/2. 

-qO/2, 

ql/2. 

0, 

0, 

0; 

q2/2. 

-ql/2. 

-qO/2, 

0, 

0, 

0; 

0, 

0, 

0, 

1, 

0, 

0; 

0, 

0, 

0, 

0, 

1, 

0; 

0, 

0, 

0, 

0, 

0, 

1] ; 
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end 


function  G  =  AHRS  G  condor (x) 


qO  =  X  (1)  ;  ql 

=  x(2) 

;  q2  = 

G  =  [  ql/2. 

q2/2. 

q3/2; 

-qO/2, 

q3/2. 

-q2/2; 

-q3/2. 

-qO/2, 

ql/2; 

q2/2. 

-ql/2. 

-qO/2; 

0, 

0, 

0; 

0, 

0, 

0; 

0, 

0, 

0]  ; 

q3  =  X  ( 4 )  ; 


end 


function  F  =  AHRS  F  condor (x,  p,  q,  r) 

%  Quaternions 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 
%  Gyro  Bias 

bgl  =  x(5);  bg2  =  x(6);  bg3  =  x(7); 


F  =  [0,  bgl/2  -  p/2,  bg2/2  -  q/2,  bg3/2  -  r/2,  ql/2,  q2/2,  q3/2; 

p/2  -  bgl/2,  0,  r/2  -  bg3/2,  bg2/2  -  q/2,  -qO/2,  q3/2, 

q2/2; 

q/2  -  bg2/2,  bg3/2  -  r/2,  0,  p/2  -  bgl/2,  -q3/2,  -qO/2, 

ql/2; 

r/2  -  bg3/2,  q/2  -  bg2/2,  bgl/2  -  p/2, 

qO/2; 


0; 

0; 


0,  q2/2,  -ql/2. 


0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

0, 

F  =  AHRS 

_F_SEAFOX(x,  p,  q. 

r,  wei,  latO) 

0]  ; 

end 


%  Quaternions 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 
%  Gyro  Bias 

bgl  =  x(5);  bg2  =  x(6);  bg3  =  x(7); 


F  =  [q0*wei* (ql*cos (latO)  -  q3*sin (latO)  )  ,  ... 

(wei*cos (latO) *q0^2) /2  +  ( 3*wei*cos ( latO ) *ql ^2 ) /2  -  ... 
wei*sin  (latO)  *ql*q3  +  (wei*cos  (latO)  *q2''2) /2  +  ... 

(wei*cos (latO) *q3^2) /2  +  bgl/2  -  p/2,  ... 

bg2/2  -  q/2  +  ql*q2*wei*cos (latO)  -  q2*q3*wei*sin (latO) ,  ... 

bg3/2  -  r/2  -  (q0''2*wei*sin  (latO)  ) /2  -  (ql''2*wei*sin  (latO)  ) /2  -  ... 
(q2^2*wei*sin (latO) ) /2  -  (3*q3^2*wei*sin (latO) ) /2  + 
ql*q3*wei*cos (latO)  ,  ... 

ql/2,  q2/2,  q3/2; 

p/2  -  bgl/2  -  (3*q0''2*wei*cos  (latO)  ) /2  -  (ql''2*wei*cos  (latO)  ) /2  -  ... 
(q2^2*wei*cos (latO) ) /2  -  (q3^2*wei*cos (latO) ) /2  -  q0*q2*wei*sin (latO) 
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-ql*wei* (qO*cos (latO)  +  q2*sin (latO) ) ,  r/2  -  bg3/2  -  ... 

(q0''2*wei*sin  (latO)  ) /2  -  (ql''2*wei*sin  (latO)  ) /2  -  ... 

(3*q2''2*wei*sin  (latO)  ) /2  -  (q3''2*wei*sin  (latO)  ) /2  -  ... 
q0*q2*wei*cos (latO)  ,  bg2/2  -  q/2  -  q0*q3*wei*cos (latO)  -  ... 
q2*q3*wei*sin (latO) ,  -qO/2,  q3/2,  -q2/2; 

q/2  -  bg2/2  +  q0*q3*wei*cos (latO)  +  qO*ql*wei*sin (latO) ,  ... 

(wei*sin  (latO)  *q0''2) /2  +  (3*wei*sin  (latO)  *ql''2) /2  +  ... 
wei*cos  (latO)  *ql*q3  +  (wei*sin  (latO)  *q2''2) /2  +  ... 

(wei*sin (latO) *q3^2) /2  +  bg3/2  -  r/2,  ... 

q2*wei* (q3*cos (latO)  +  ql*sin (latO) ) ,  ... 

(wei*cos (latO) *q0^2) /2  +  (wei*cos ( latO ) *ql ^2 ) /2  +  wei*sin (latO) *ql*q3  + 

(wei*cos  (latO)  *q2''2) /2  +  (3*wei*cos  (latO)  *q3''2) /2  -  bgl/2  +  ... 
p/2,  -q3/2,  -qO/2,  ql/2; 

(3*wei*sin  (latO)  *q0''2) /2  -  wei*cos  (latO)  *q0*q2  +  ... 

(wei*sin (latO) *ql^2) /2  +  (wei*sin (latO) *q2^2) /2  +  ... 

(wei*sin  (latO)  *q3''2) /2  -  bg3/2  +  r/2,  q/2  -  ... 
bg2/2  -  ql*q2*wei*cos (latO)  +  qO*ql*wei*sin (latO) ,  ... 

bgl/2  -  p/2  -  (q0^2*wei*cos (latO) ) /2  -  (ql^2*wei*cos (latO) ) /2  -  ... 
(3*q2''2*wei*cos  (latO)  ) /2  -  (q3''2*wei*cos  (latO)  ) /2  +  ... 
q0*q2*wei*sin (latO) ,  -q3*wei* (q2*cos (latO)  -  qO*sin (latO) ) ,  ... 


q2/2. 

-ql/2, 

■qO/2 

r 

0, 

0, 

0,  0, 

0, 

0, 

0; 

0, 

0, 

0,  0, 

0, 

0, 

0; 

0, 

0, 

0,  0, 

0, 

0, 

0]  ; 

end 

function  H  =  AHRS  H(x,  x2,  p,  q,  r,  g,  ax,  ay,  az) 
qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 
bgl  =  x(5);  bg2  =  x(6);  bg3  =  x(7); 
vn  =  x2  ( 4 )  ;  ve  =  x2  ( 5 )  ;  vd  =  x2  ( 6 )  ; 

H  =  [ (bg3  -  r)*(2*ql*vd  +  2*q0*ve  -  2*q3*vn)  -  (bg2  -  q)*(2*q0*vd  -  ... 
2*ql*ve  +  2*q2*vn)  +  2*ax*q0  +  2*ay*q3  -  2*az*q2  -  2*g*q2,  ... 

(bg2  -  q)*(2*ql*vd  +  2*q0*ve  -  2*q3*vn)  +  (bg3  -  r)*(2*q0*vd  -  ... 

2*ql*ve  +  2*q2*vn)  +  2*ax*ql  +  2*ay*q2  +  2*az*q3  +  2*g*q3,  ... 

(bg3  -  r)*(2*q3*vd  +  2*q2*ve  +  2*ql*vn)  -  (bg2  -  q)*(2*q3*ve  -  ... 

2*q2*vd  +  2*q0*vn)  -  2*ax*q2  +  2*ay*ql  -  2*az*q0  -  2*g*q0,  ... 

2*ay*q0  -  (bg3  -  r) * (2*q3*ve  -  2*q2*vd  +  2*q0*vn)  -  2*ax*q3  -  ... 
(bg2  -  q)*(2*q3*vd  +  2*q2*ve  +  2*ql*vn)  +  2*az*ql  +  2*g*ql, 

0,  ve*(2*q0*ql  -  2*q2*q3)  -  vd*  (q0''2  -  ql^2  -  q2'^2  +  q3^2)  - 

vn* (2*q0*q2  +  2*ql*q3),  ve* (q0^2  -  ql^2  +  q2^2  -  q3^2)  +  vd*(2*q0*ql  + 

2*q2*q3)  -  vn*(2*q0*q3  -  2*ql*q2); 

(bgl  -  p)*(2*q0*vd  -  2*ql*ve  +  2*q2*vn)  -  (bg3  -  r)*(2*q3*ve  -  ... 

2*q2*vd  +  2*q0*vn)  -  2*ax*q3  +  2*ay*q0  +  2*az*ql  +  2*g*ql,  ... 

2*ax*q2  -  (bg3  -  r)*(2*q3*vd  +  2*q2*ve  +  2*ql*vn)  -  (bgl  -  ... 

p)*(2*ql*vd  +  2*q0*ve  -  2*q3*vn)  -  2*ay*ql  +  2*az*q0  +  2*g*q0,  ... 

(bgl  -  p)*(2*q3*ve  -  2*q2*vd  +  2*q0*vn)  +  (bg3  -  r)*(2*q0*vd  -  ... 

2*ql*ve  +  2*q2*vn)  +  2*ax*ql  +  2*ay*q2  +  2*az*q3  +  2*g*q3,  ... 

(bgl  -  p)*(2*q3*vd  +  2*q2*ve  +  2*ql*vn)  -  (bg3  -  r)*(2*ql*vd  +  ... 

2*q0*ve  -  2*q3*vn)  -  2*ax*q0  -  2*ay*q3  +  2*az*q2  +  2*g*q2,  ... 

vd* (q0^2  -  ql^2  -  q2^2  +  q3^2)  -  ve*(2*q0*ql  -  2*q2*q3)  +  ... 
vn*(2*q0*q2  +  2*ql*q3),  0,  vd*(2*q0*q2  -  2*ql*q3)  -  vn*  (q0''2  +  ... 
ql^2  -  q2^2  -  q3^2)  -  ve* (2*q0*q3  +  2*ql*q2); 

(bg2  -  q)*(2*q3*ve  -  2*q2*vd  +  2*q0*vn)  -  (bgl  -  p)*(2*ql*vd  +  ... 
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2*q0*ve  -  2*q3*vn)  +  2*ax*q2  -  2*ay*ql  +  2*az*q0  +  2*g*q0,  ... 

(bg2  -  q)*(2*q3*vd  +  2*q2*ve  +  2*ql*vn)  -  (bgl  -  p)*(2*q0*vd  -  ... 

2*ql*ve  +  2*q2*vn)  +  2*ax*q3  -  2*ay*q0  -  2*az*ql  -  2*g*ql,  ... 

2*ax*q0  -  (bg2  -  q)*(2*q0*vd  -  2*ql*ve  +  2*q2*vn)  -  (bgl  -  ... 

p)*(2*q3*vd  +  2*q2*ve  +  2*ql*vn)  +  2*ay*q3  -  2*az*q2  -  2*g*q2,  ... 

(bgl  -  p)*(2*q3*ve  -  2*q2*vd  +  2*q0*vn)  +  (bg2  -  q)*(2*ql*vd  +  ... 

2*q0*ve  -  2*q3*vn)  +  2*ax*ql  +  2*ay*q2  +  2*az*q3  +  2*g*q3,  ... 

vn*(2*q0*q3  -  2*ql*q2)  -  vd*(2*q0*ql  +  2*q2*q3)  -  ve*  (q0''2  -  ... 
ql^2  +  q2^2  -  q3^2),  vn* (q0^2  +  ql^2  -  q2^2  -  q3^2)  -  vd* (2*q0*q2 

-  2*ql*q3)  +  ve*(2*q0*q3  +  2*ql*q2),  0; 

-  (2*  (q0''2*q3  +  2*q0*ql*q2  -  ql^2*q3  +  q2^2*q3  +  q3 ^ 3 )  )  /  (  (  ( 4 *  ( qO *q3 

ql*q2) ^2) / (q0^2  +  ql^2  -  q2^2  -  q3^2)^2  +  l)*(q0^2  +  ql^2  -  q2^2  - 

q3^2)^2),  -(2*(-  q0^2*q2  +  2*q0*ql*q3  +  ql^2*q2  +  q2^3  +  ... 
q2*q3-"2)  )  /  (  (  (4*  (q0*q3  +  ql*q2)  "2)  /  (q0'"2  +  ql"2  -  q2''2  -  q3^2)-"2  + 

l)*(q0^2  +  ql^2  -  q2^2  -  q3^2)^2),  (2*(q0^2*ql  +  2*q0*q2*q3  +  ql^3 

+  ql*q2^2  -  ql *q3 ^2 ) ) / ( ( ( 4 * ( q0*q3  +  ql*q2 ) ^2 ) / (q0^2  +  ql^2  -  q2^2 

-  q3^2)''2  +  l)*(q0^2  +  ql-'2  -  q2^2  -  q3''2)^2),  (2*(q0-'3  +  q0*ql''2  - 

q0*q2''2  +  q0*q3-'2  +  2 *ql *q2 *q3 )  )  /  (  (  ( 4 *  ( qO *q3  +  ql *q2  ) '^2  )  /  (qO ^2  +  .. 
ql^2  -  q2^2  -  q3^2)^2  +  l)*(q0^2  +  ql^2  -  q2^2  -  q3^2)^2),  0,  0,  0] 

end 


function  q  =  e2q(phi,  theta,  psi) 

%  Form  Rotation  Matrix 

R_psi  =  [cos (psi)  sin (psi)  0;  -sin (psi)  cos (psi)  0;  0  0  1]; 

R_theta  =  [cos (theta)  0  -sin (theta) ;  0  1  0;  sin (theta)  0  cos (theta) ] ; 
R_phi  =[10  0;  0  cos (phi)  sin (phi);  0  -sin (phi)  cos  (phi) ]; 

R  n2b  =  R  phi*R  theta*R  psi; 

%  Extract  Quaternions 
q0=sqrt (1+trace (R_n2b) ) /2; 
ql= (R_n2b (2,3) -R_n2b (3,2) ) / (4*q0) ; 
q2= (R_n2b (3,1) -R_n2b (1,3) ) / ( 4 *q0 )  ; 
q3= (R_n2b (1, 2) -R_n2b (2,1))/ (4*q0)  ; 

q  =  [qO;  ql;  q2 ;  q3]; 
end 


function  [phi,  theta,  psi]  =  q2e (x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 


%  Compute  Euler  Angles 

phi  =  atan2  (2*  (q2*q3+q0*ql)  ,  (q0''2-ql''2-q2''2+q3''2)  )  ; 
theta  =  asin (-2* (ql*q3-q0*q2) ) ; 

psi  =  mod  (atan2  (2*  (ql*q2+q0*q3)  ,  (q0''2+ql''2-q2''2-q3''2)  )  ,  2*pi)  ; 
end 

end 
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c. 


STANDARD  UNSCENTED  KALMAN  FILTER  MATLAB  CODE 


1,  Measurement  Model  One  Implementation 

function  X_HAT  =  UKFl (u) 

%%  SPHERICAL  SIMPLEX  UNSCENTED  KALMAN  FILTER 
%  Measurement  Model  I 
%  LT  Steven  Terjesen 
%  September  2014 

%  This  estimator  is  built  for  use  in  MATLAB  Simulink.  There  are  38 
inputs 

%  required  and  can  be  run  in  Simulink  with  an  "Interpolated  MATLAB 
%  Function"  block.  The  first  13  inputs  are  the  vehicle  data:  Course 
Over 

%  Ground,  Speed  Over  Ground,  Accelerometer  Measurements,  Gyro 
Measurements , 

%  GPS  Measurements  (In  LTP  XNorth-YEast-ZDown) ,  Heading  Measurement, 
and 

%  Vertical  Velocity  (zeroed  out  for  SEAFOXII  data) .  Inputs  14  through 
34 

%  are  the  Process  Noise  and  Measurement  Noise  diagonal  elements.  These 
%  elements  are  not  hard  coded  to  allow  for  easier  tuning.  Inputs  35 
%  through  37  are  the  N-E-D  accelerations  in  the  LTP  frame.  Input  38  is 
a 

%  toggle  for  selecting  whether  the  data  is  from  Condor  or  SEAFOX  II. 


%%  System  Inputs 
%  Measurement  inputs 


CoG  =  u ( 1 ) ; 

%GPS 

Course  Over  Ground 

speed  =  u(2); 

%GPS 

Speed  Over  Ground 

fx  =  u  ( 3 )  ;  f  y 

=  u(4) 

;  f  z  =  u  ( 5 )  ; 

%IMU 

Accelerations 

p  =  u  (  6 )  ;  q  = 

u(7)  ; 

r  =  u  ( 8 )  ; 

%IMU 

Angular  Rates 

N  sat  =  u ( 9 ) ; 

E  sat 

=  u(10);  D  sat  =  u(ll); 

%GPS 

Position  (NED) 

heading  =  u ( 12 ) ; 

%GPS 

Heading 

Vd  sat  =  u(13) 

'  ; 

%GPS 

LTP  Down  Velocity 

%  Process  and  Measurement  Noise  Matrices 
R  =  diag (u (14 : 16) ) ; 

R2  =  diag (u  (17:22)); 


%AHRS  Measurement  Noise 
%INS  Measurement  Noise 


SIM  =  u(38);  %Condor(l)  or 

SEAFOXII (2) 

%  Simulation  Selector 


if  SIM  ==  1 

Qv  =  diag (u (23 : 25) ) ; 
Condor 

Qv2  =  diag (u (29 : 31) ) ; 
Condor 
else 

Qv  =  diag (u (23 : 2 8 ) ) ; 
SEAFOX 


%AHRS  Process  Noise 
%INS  Process  Noise 

%AHRS  Process  Noise 
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INS  Process  Noise 


Qv2  =  diag (u (2 9 : 34 ) )  ; 

SEAFOX 

Vd_sat  =  0; 

q, 

o 

on 

q, 

o 

D_sat  =  0; 

Measurement 

q, 

o 

set 

q, 

o 

end 

%  GPS  Accelerations 

ax_gps  =  u(35);  ay_gps  =  u(36);  az 


g, 
o 

%No  Vertical  Velocity 
measurement  available 

SEAFOX  II,  set  Vd=0. 
%No  Altitude 

available  on  SEAFOXII, 

D  sat  =  0; 


=  u(37);  %GPS  Accelerations  as 
%  calculated  from  3rd 
%  Order  Filter 


%%  Initialization 

persistent  x  bar  HeadingO  HI  H2  ii  j j  psiO  H3  H4  ... 

Px  Px2  kk  dt  g  nn  nx  nxa  nQ  nR  n  wc  wm  x  bar2  nx2  nxa2  n2  nQ2  nR2  wc2 
wm2  eta  eta2  wei  latO 

%  Allows  time  for  Condor  To  steady  out  during  live  testing 
if  isempty(kk) 
kk  =  0; 

end 

if  kk  <  1  &&  SIM==1 

X_HAT  =  zeros (16,1); 

else 

if  isempty(x  bar) 

%  Miscellaneous 
dt  =  0.01; 
g  =  9.8; 

[m/s''2  ] 

wei  =  7.292115*10^-5; 
lat0=  0.639268394832413; 

%lon0  =  -2.115435878466264 
nn=l ; 

Initializer 

%  AHRS  Initialization 
%  Euler  Angle  Initialization 

rx  =  ax  gps*cos (heading) +ay  gps*sin (heading) ; 
ry  =  -ax_gps*sin (heading) +ay_gps*cos (heading)  ; 
rz  =  az_gps-g; 

theta  0  =  atan((-rx*rz  -  fx*sqrt  (rx''2+rz''2-fx''2)  )  /  (fx^2-rz''2)  )  ; 
r_theta  =  rx*sin (theta_0)  +  rz*cos (theta_0) ; 
fc  =  f y- speed* r; 

phi  0  =  atan((r  theta*ry  +  fc*sqrt (ry^2+r  theta^2-fc^2) ) / (fc^2- 
r_theta^2 ) ) ; 
psi  0=heading; 


%Filter  dt  [sec] 
%Gravity  Constant 

%Sidereal  Rate  [rad/s] 
%Origin  of  LTP  [rad] 

%Heading  Count 
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%  Euler  Angles  to  Quaternions 
q_0  =  e2q(phi_0,  theta_0,  psi_0); 
%  Gyro  Bias  Initial  Values 
bg_0  =  [ 0 ;  0 ;  0 ] ; 

%  Inital  State  Vector 


X 

bar 

=  [q_0; 

bg_0 ] ; 

q, 

o 

Ini 

tial  Covariance  Estimate 

Px 

= 

diag ( [ le 

-5*ones(l,4)  le-3*ones  (1, 3) ] )  ; 

%INS 

Initial! 

zation 

q, 

o 

Pos 

ition  & 

Velocity  Initialization 

X 

0  = 

N  sat; 

y_ 

0  = 

E  sat; 

z 

0  = 

D  sat; 

vn 

0 

=  speed* 

cos (CoG) ; 

ve 

0 

=  speed* 

sin (CoG) ; 

vd 

0 

=  Vd  sat 

r 

%  Accelerometer  Initial  Bias  Estimate 
ba_0  =  [ 0 ;  0 ;  0 ]  ; 

%  Initial  State  Vector 

x_bar2  =  [x_0;  y_0;  z_0;  vn_0;  ve_0;  vd_0;  ba_0] ; 

%  Initial  Covariance  Estimate 

Px2  =  diag ( [le-5*ones (1, 3) ,  le-5*ones (1, 3) ,  le-3*ones (1, 3) ] ) ; 


%  AHRS  SIGMA  Weights  Initialization 
nx  =  length (x  bar); 
nQ  =  length (diag (Qv) ) ; 
nR  =  length (diag (R) ) ; 

States 

nxa  =  nx+nQ+nR; 

States 

n  =  2*nxa+l; 

Iterations 
alpha  =  le-3; 

Factor 
beta  =  2; 

PDF 

kappa  =  0 ; 

Scaling 

q, 

o 

lamda  =  alpha^2 * (nxa+kappa) -nxa; 
wm  =  (1/(2* (nxa+ lamda) ) ) *ones (n, 1) ; 
wc  =  wm; 

wm ( 1 )  =  lamda/ (nxa+lamda) ; 

Weight 

wc  ( 1 )  =  wm  ( 1 )  +  ( l-alpha''2+beta)  ; 
Weight 

eta  =  sqrt (nxa+lamda) ; 

q, 

o 


%  Number  of  AHRS  States 
%  Process  Noise  States 
%  Measurement  Noise 

%  Total  Augmented 

%  Number  of 

%  Tunable  Scaleing 

%  beta  =  2  for  Gaussian 

%  Tunable  Secondary 

Factor . 

%  Weighting  Factor 
%  Measurement  Weights 
%  Covariance  Weights 
%  Zeroth  Measurement 

%  Zeroth  Covariance 

%  Covariance  Weighting 
Factor 


%  INS  SIGMA  Weights  Initialization 
nx2  =  length (x  bar2); 
nQ2  =  length (diag (Qv2 )) ; 
nR2  =  length (diag (R2 )) ; 

States 


%  Number  of  INS  States 
%  Process  Noise  States 
%  Measurement  Noise 
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nxa2  =  nx2+nQ2+nR2 ; 

States 

n2  =  2*nxa2+l; 
alpha2  =  le-3; 
beta2  =  2; 

PDF 

k;appa2  =  0 ; 

Scaling 

q, 

o 

lamda2  =  alpha2 ^^2 *  (nxa2  +  k;appa2 ) -nxa2 ; 
wm2  =  (1/ (2* (nxa2+lamda2 ) ) ) *ones (n2, 1) ; 
wc2  =  wm2 ; 

wm2 ( 1 )  =  lamda2 / (nxa2+lamda2 ) ; 

Weight 

wc2(l)  =  wm2 ( 1 )  +  (l-alpha2^2+beta2) ; 

Weight 

eta2  =  sqrt (nxa2+lamda2 ) ; 

q, 

o 


%  Total  Augmented 

%  Total  Iterations 
%  Tunable,  0<alpha<l 
%  beta  =  2  for  Guassian 

%  Tunable  Secondary 

Factor . 

%  Weighting  Factor 
%  Measurement  Weights 
%  Covariance  Weights 
%  Zeroth  Measurement 

%  Zeroth  Covariance 

%  Covariance  Weighting 
Factor 


end 

%%  AHRS  ESTIMATOR 

%  Build  Augmented  State  Vector 

X  =  zeros (nxa,  n) ; 

xa  =  [x  bar;  zeros (nxa-nx, 1 )] ; 

%  Build  Augmented  Covariance  Matrix 
Pxa  =  [Px  zeros (nx,nQ)  zeros (nx, nR) ; 

zeros (nQ,nx)  Qv  zeros (nQ, nR) ; 
zeros (nR,nx)  zeros (nR,nQ)  R]  ; 

%  Build  2n+l  Sigma  Vectors 
c  =  eta*Pxa; 
for  k  =  1 : nxa+1 
if  k  ==  1 

X ( : , k)  =  xa; 

else 

X ( : , k)  =  xa  +  c(k-l,:)'; 
X(:,k+nxa)  =  xa  -  c(k-l,:)'; 

end 

end 

%  Time  Update 
Xx  =  zeros (nx,  n) ; 

X  bar  =  zeros (nx,  1); 
for  k  =  l:n 


%  Gyro  Model  w_bi (true)  =  w_bi (measured)  -  bias  -  noise 
w_bi_b  =  [p-X  ( 5 ,  k) -X  (  8 ,  k)  ;  q-X  (  6,  k) -X  (  9,  k)  ;  r-X(7,  k)-X(10,k)]; 

if  SIM  ==  1 

%  No  Sidereal  Rate 
w  bt  =  w  bi  b; 

else 
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%  Sidereal  Rate  Included 
R_t2b  =  rot_t2b (X ( 1 : 4 , k) ) ; 

w_bt  =  w_bi_b  -  R_t2b* [wei*cos (latO) ;  0;  -wei*sin (latO) ] 

end 


%  Quaternion  Update 

wl  =  w  bt(l);  w2  =  w  bt(2);  w3  =  w  bt(3); 

Q  kpl  =  X(l:4,  k)  +  (dt/2)*[0,  -wl,  -w2,  -w3; 


wl,  0,  w3, 

w2 ,  -w3,  0, 

w3,  w2 ,  -wl. 


-w2 ; 
wl ; 

0]*X(1:4, 


k)  ; 


%  Quaternion  Normalization 
Q  kpl  n  =  Q  kpl/sqrt(Q  kpl'*Q  kpl); 

%  Gyro  Bias  Update 
if  SIM  ==  1 

%  Constant  Bias 
bg  =  X  ( 5  :  7 ,  k)  ; 

else 

%  Random  Walk  Bias 

bg  =  X(5:7,k)  +  dt*X(ll:13,k); 

end 


%  Rebuild  the  State  Vector  for  Each  Iteration 
Xx(:,k)  =  [Q_kpl_n;  bg] ; 


%  Calculated  the  Weighted  Mean  of  the  State  Vector 

X  bar  =  X  bar  +  wm ( k) *Xx ( : , k) ; 

end 

%  Calculate  Error  Covariance 
Px=  zeros (nx,  nx) ; 
for  k  =  1 : n 

Px  =  Px  +  wc ( k) * ( (Xx ( : , k)  -  x  bar ) * (Xx ( : , k)  -  x  bar)'); 

end 


%  Non-Linear  Measurement  Process  Equations 
Y  =  zeros (nR,  n) ; 
y_bar  =  zeros (nR, 1); 

Heading  =  zeros (l,n); 
if  isempty(Hl) 

HI  =  zeros ( 1 , n) ; 

H2  =  zeros ( 1 , n) ; 

Headings  =  zeros (l,n); 
ii  =  zeros ( 1 , n) ; 

end 

for  k  =  l:n 

if  SIM  ==  1 

noise  R  =  X(lI:I3,k); 

else 

noise  R  =  X(14:16,k); 

end 


%  Pre-fill 
%  Pre-fill 
%  Pre-fill 

%  Pre-fill 
%  Pre-fill 
%  Pre-fill 
%  Pre-fill 
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%  Quaternion  to  Euler  Angles 

qO  =  Xx(l,k;);  ql  =  Xx(2,k;);  q2  =  Xx(3,k;);  q3  =  Xx(4,k;); 

phi  =  atan2  (2*  (q2*q3+q0*ql)  ,  q0''2-ql''2-q2''2+q3''2)  +  noise  R  ( 1 )  ; 

theta  =  asin (-2* (ql*q3-q0*q2) ) +  noise_R(2); 

psi  =  mod  (atan2  (2*  (ql*q2+q0*q3)  ,  q0''2+ql''2-q2''2-q3''2)  +  noise  R(3), 
2*pi) ; 

%  Unwrap  Heading  Angle  From  [0,  2*pi]  Range  to  Avoid  Jumps 
if  nn  ==  1 

HI (k)  =  psi; 

HeadingO(k)  =  Hl(k); 

Heading (k)  =  Hl(k); 

else 

H2 (k)  =  psi; 

if  (H2 (k) -HI (k) )  <=  -100*pi/180 
ii(k)  =  ii(k)+l; 

end 

if  (H2 (k) -HI (k) )  >=  100*pi/180 
ii (k)  =  ii (k) -1; 

end 

Heading (k)  =  HeadingO (k) + ( (H2 (k) +2*pi*ii (k) )  -  HeadingO ( k) ) ; 

HI (k)  =  H2 (k) ; 

end 

%  Calculated  Observations 
Y(;,k)=[phi;  theta;  Heading (k)]; 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm ( k) *Y ( : , k) ; 
end 

%  Estimated  Observation  Covariance 
Py  =  zeros (nR, nR) ; 
for  k  =  1 : n 

Py  =  Py  +  wc ( k) * ( ( Y ( : , k)  -  y_bar) * (Y ( : , k)  -  y_bar) ' ) ; 

end 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx, nR) ; 
for  k  =  l:n 

Pxy  =  Pxy  +  wc ( k) * ( (Xx ( : , k)  -  x  bar) * (Y ( : , k)  -  y  bar)'); 

end 

%  Measurement  Method  1  (Accelerometer-GPS-Compass  Fusion) 

%  Rotate  GPS  XYZ  Acceleration  about  Heading 
rx  =  ax  gps*cos (heading) lay  gps*sin (heading) ; 
ry  =  -ax  gps*sin (heading) +ay  gps*cos (heading) ; 
rz  =  az_gps-g; 

%  Computer  Pitch 

theta  =  atan((-rx*rz  -  fx*sqrt (rx^2  +  rz^2-fx^2 )  )  /  (fx^2-rz^2) ) ; 

%  Compute  Roll 

r_theta  =  rx*sin (theta)  +  rz*cos (theta) ; 

fc  =  fy-(norm(x  bar2 (4 : 6) ) ) *r;  %  Added  Coriolis  Term 
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phi  =  atan((r  theta*ry  +  fc*sqrt (ry^2+r  theta^2-fc^2 ) )  / 

r_theta''2 )  )  ; 

%  Heading  Unwrap  from  [0,  2*pi] 
if  nn  ==  1 

H3  =  heading; 

H4  =  0; 
psiO  =  H3; 
psi  m  =  H3; 

j  j=0; 

else 

H4  =  heading; 

if  (H4-H3)  <=  -100*pi/180 

jj  =  jj+1; 

end 

if  (H4-H3)  >=  100*pi/180 

jj  =  jj-1; 

end 

psi_m  =  psi0+ ( (H4+2*pi* j j )  -  psiO); 

H3  =  H4; 

end 

%  Measurement  Vector 
Z  =  [phi;  theta;  psi_m] ; 

%  Kalman  Filter  Equations 
K  =  Pxy/Py; 

X  bar  =  X  bar  +  K* (Z  -  y  bar)  ; 

%  Covariance  Corrections 
Px  =  Px  -  K*Py*K'; 

nn  =  2 ; 

%%  INS  ESTIMATOR 

%  Build  Augmented  State  Vector 
X  =  zeros (nxa2,  n2); 
xa  =  [x  bar2;  zeros (nxa2-nx2 , 1 )] ; 

%  Build  Augmented  Covariance  Matrix 
Pxa2  =  [Px2  zeros (nx2 , nQ2 )  zeros (nx2 , nR2 ) ; 

zeros (nQ2 , nx2 )  Qv2  zeros (nQ2 , nR2 ) ; 
zeros (nR2 , nx2 )  zeros (nR2 , nQ2 )  R2 ]  ; 

%  Construct  2n+l  SIGMA  Vectors 
c  =  eta2*Pxa2; 
for  k  =  l:nxa2+l 
if  k  ==  1 

X ( : , k)  =  xa; 

else 

X(:,k)  =  xa  +  c(k-l,:)'; 

X(:,k+nxa2)  =  xa  -  c(k-l,:)'; 

end 

end 


%  Kalman 
%  Kalman 


(fc"2- 


Gain 

Correction 
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%  Time  Update 

Xx  =  zeros (nx2,  n2); 

X  bar2  =  zeros (nx2,  1); 
for  k  =  1 : n2 
%  Rotation  Matrix 
R  t2b  =  rot  t2b(x  bar (1:4)); 

R  b2t  =  R  t2b' ; 

%  Position  Update 

P_kpl  =  X(l:3,k)  +  dt*[X(4:5,k)  ;  -X(6,k)]; 

%  Velocity  and  Bias  Update 
if  SIM  ==  1 
%  No  Sidereal 

V  kpl  =  X(4:6,k)  +  dt* (R  b2t* ( [fx;  fy;  fz]  -  X( 

X(10:12,  k) )  +  [0;  0;  g] ) ; 
ba_kpl  =  X(7:9,k); 

else 

%  Sidereal  Included 

V  kpl  =  X(4:6,k)  +  dt* (R  b2t* ( [fx;  fy;  fz]  -  X( 

X(10:12,  k))  +  [0;  0;  g]  -  2*cross([wei 
0;  -wei*sin (latO) ] ,  X(4:6,k))); 
ba_kpl  =  X(7:9,k)  +  dt*X (13 : 15, k) ; 

end 

%  States 

Xx ( : , k)  =  [P  kpl;  V  kpl;  ba  kpl]; 

%  Calculated  Mean 

X  bar2  =  x  bar2  +  wm2 ( k) *Xx ( : , k) ; 
end 

%  Predicted  Error  Covariance 
Px2=  zeros (nx2,  nx2); 
for  k  =  1 : n2 

Px2  =  Px2  +  wc2 ( k) * ( (Xx ( : , k)  -  x  bar2 ) * (Xx ( : , k) 

end 

%  Measurement  Equations 
Y  =  zeros (nR2,  n2); 
y_bar  =  zeros (nR2 , 1 ) ; 
for  k  =  1 : n2 

if  SIM  ==  1 

%  Calculated  Observations 
Y(:,k)=Xx(l:6,k)+X(13:18,k)  ; 

else 

%  Calculated  Observations 
Y(:,k)=Xx(l:6,k)+X(16:21,k)  ; 

end 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm2 ( k) *Y ( : , k) ; 


%  Pre-fill 
%  Pre-fill 


:9,k)  - 


:9,k)  -  .  .  . 

cos ( latO ) ; 


-  X  bar2) ' ) 


%  Pre-fill 
%  Pre-fill 
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end 


%  Estimated  Observation  Covariance 
Py  =  zeros (nR2 , nR2 ) ; 
for  k  =  1 : n2 

Py  =  Py  +  wc2 ( k) * ( ( Y ( : , k)  -  y_bar) * (Y ( : , k)  -  y_bar) ' ) ; 

end 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx2 , nR2 ) ; 
for  k  =  1 : n2 

Pxy  =  Pxy  +  wc2 ( k) * ( (Xx ( : , k)  -  x  bar2 ) * ( Y ( : , k)  -  y  bar)'); 

end 

%  Measurements 

Z  =  [N_sat;  E_sat;  D_sat;  speed*cos (CoG) ;  speed*sin (CoG) ;  Vd_sat] ; 

%  Kalman  Filter  Equations 

K  =  Pxy/Py;  %  Kalman  Gain 

%  Kalman  State  Corrections 
X  bar2  =  x  bar2  +  K* (Z  -  y  bar) ; 

%  Covariance  Correction 
Px2  =  Px2  -  K*Py*K'; 

X  HAT  =  [x  bar (1:4);  x  bar2(l:6);  x  bar (5: 7);  x  bar2(7:9)]; 
end 

kk=kk+l; 

end 

function  R  t2b  =  rot  t2b(x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

R_t2b  =  [q0^2+ql^2-q2^2-q3^2  2* (ql*q2+q0*q3)  2*(ql*q3-q0*q2); 

2*  (q2*ql-q0*q3)  qO ^^2 -ql ''2+q2 ^^2 -q3 ''2  2*(q2*q3+q0*ql); 

2* (q3*ql  +  q0*q2)  2  * ( q3 *q2 -qO *ql )  qO ^2 -ql ^2 -q2 ^2  +  q3 ^2 ] ; 

end 

function  q  =  e2q(phi,  theta,  psi) 

%  Form  Rotation  Matrix 

R_psi  =  [cos (psi)  sin (psi)  0;  -sin (psi)  cos (psi)  0;  0  0  1]; 

R_theta  =  [cos (theta)  0  -sin  (theta);  0  1  0;  sin  (theta)  0  cos (theta) ] 
R_phi  =[10  0;  0  cos (phi)  sin (phi);  0  -sin (phi)  cos  (phi)]; 

R  n2b  =  R  phi*R  theta*R  psi; 

%  Extract  Quaternions 
q0=sqrt (1+trace (R_n2b) ) /2; 
ql= (R_n2b (2,3) -R_n2b (3,2) ) / (4*q0) ; 
q2= (R_n2b (3,1) -R_n2b (1,3) ) / ( 4 *q0 )  ; 
q3= (R_n2b (1, 2) -R_n2b (2,1))/ (4*q0)  ; 

q  =  [qO;  ql;  q2 ;  q3]; 
end 
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2. 


Measurement  Model  Two  Implementation 


function  X_HAT  =  UKF2 (u) 

%%  UNSCENTED  KALMAN  FILTER 
%  Measurement  Model  2 
%  LT  Steven  Terjesen 
%  September  2014 

%  This  estimator  is  built  for  use  in  MATLAB  Simulink.  There  are  39 
inputs 

%  required  and  can  be  run  in  Simulink  with  an  "Interpolated  MATLAB 
%  Function"  block.  The  first  13  inputs  are  the  vehicle  data:  Course 
Over 

%  Ground,  Speed  Over  Ground,  Accelerometer  Measurements,  Gyro 
Measurements , 

%  GPS  Measurements  (In  LTP  XNorth-YEast-ZDown) ,  Heading  Measurement, 
and 

%  Vertical  Velocity  (zeroed  out  for  SEAFOXII  data) .  Inputs  14  through 
35 

%  are  the  Process  Noise  and  Measurement  Noise  diagonal  elements.  These 
%  elements  are  not  hard  coded  to  allow  for  easier  tuning.  Inputs  36 
%  through  38  are  the  N-E-D  accelerations  in  the  LTP  frame.  Input  39  is 
a 

%  toggle  for  selecting  whether  the  data  is  from  Condor  or  SEAFOX  II. 

%%  System  Inputs 
%  Measurement  inputs 


CoG  =  u ( 1 ) ; 

%GPS 

Course  Over  Ground 

speed  =  u(2); 

%GPS 

Speed  Over  Ground 

fx  =  u (3) ;  fy 

=  u(4) 

;  f  z  =  u  ( 5 )  ; 

%IMU 

Accelerations 

p  =  u  ( 6 )  ;  q  = 

u(7)  ; 

r  =  u  ( 8 )  ; 

%IMU 

Angular  Rates 

N  sat  =  u ( 9 ) ; 

E  sat 

=  u(10);  D  sat  =  u(ll); 

%GPS 

Position  (NED) 

heading  =  u ( 12 

) ; 

%GPS 

Heading 

Vd  sat  =  u(13) 

r 

%GPS 

LTP  Down  Velocity 

%  Process  and  Measurement  Noise 
R  =  diag (u  ( 14  : 17 ) )  ; 

R2  =  diag (u ( 1 8 : 23 ) )  ; 

SIM  =  u  (39)  ; 

SEAFOXII  (2) 

q, 

o 

if  SIM  ==  1 

Qv  =  diag (u (24 : 2  6) )  ; 

Condor 

Qv2  =  diag (u ( 30 : 32 ) ) ; 

Condor 

else 

Qv  =  diag (u (24 : 29) ) ; 

SEAFOX 

Qv2  =  diag (u ( 30 : 35 ) ) ; 

SEAFOX 

Vd  sat  =  0; 


Matrices 

%AHRS  Measurement  Noise 
%INS  Measurement  Noise 

%Condor(l)  or 
Simulation  Selector 

%AHRS  Process  Noise 
%INS  Process  Noise 

%AHRS  Process  Noise 
%INS  Process  Noise 
%No  Vertical  Velocity 
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measurement  available 

SEAFOX  II,  set  Vd=0. 
%No  Altitude 

available  on  SEAFOXII, 

D  sat  =  0; 


%  GPS  Accelerations 

ax_gps  =  u(36);  ay_gps  =  u(37);  az_gps  =  u(38);  %GPS  Accelerations  as 

%  calculated  from  3rd 
%  Order  Filter 

%%  Initialization 

persistent  x  bar  HeadingO  HI  H2  ii  j j  psiO  H3  H4  ... 

Px  Px2  kk  dt  g  nn  nx  nxa  nQ  nR  n  wc  wm  x  bar2  nx2  nxa2  n2  nQ2  nR2  wc2 
wm2  eta  eta2  wei  latO 

%  Allows  time  for  Condor  To  steady  out  during  live  testing 
if  isempty(kk) 
kk  =  0; 

end 

if  kk  <  1  &&  SIM==1 

X_HAT  =  zeros (16,1); 

else 

if  isempty(x  bar) 

%  Miscellaneous 
dt  =  0.01; 
g  =  9.8; 

[m/s^2 ] 

wei  =  7.292115*10^-5; 
lat0=  0.639268394832413; 

%lon0  =  -2.115435878466264 
nn=l ; 

Initializer 

%  AHRS  Initialization 
%  Euler  Angle  Initialization 

rx  =  ax  gps*cos (heading) +ay  gps*sin (heading) ; 
ry  =  -ax  gps*sin (heading) +ay  gps*cos (heading) ; 
rz  =  az_gps-g; 

theta  0  =  atan((-rx*rz  -  fx*sqrt  (rx''2+rz''2-fx''2)  )  /  (fx^2-rz''2)  )  ; 
r_theta  =  rx*sin (theta_0)  +  rz*cos (theta_0) ; 
fc  =  f y- speed* r; 

phi  0  =  atan((r  theta*ry  +  fc*sqrt  (ry''2+r  theta''2-fc''2)  )  /  (fc''2- 
r_theta^2 ) )  ; 
psi  0=heading; 

%  Euler  Angles  to  Quaternions 
q_0  =  e2q(phi_0,  theta_0,  psi_0); 

%  Gyro  Bias  Initial  Values 


%Filter  dt  [sec] 
%Gravity  Constant 

%Sidereal  Rate  [rad/s] 
%Origin  of  LTP  [rad] 

%Heading  Count 


on 

g, 

o 

D_sat  =  0; 
Measurement 

g, 

o 

set 


end 
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bg_0  =  [ 0 ;  0 ;  0 ]  ; 

%  Inital  State  Vector 
x_bar  =  [q_0;  bg_0] ; 

%  Initial  Covariance  Estimate 

Px  =  diag ( [le-5*ones (1, 4)  le-3*ones (1, 3) ] ) ; 

%INS  Initialization 

%  Position  &  Velocity  Initialization 


II 

o 

1 

X 

N  sat; 

II 

O 

E  sat; 

II 

o 

N 

D  sat; 

vn  0 

=  speed*cos 

(CoG) ; 

ve  0 

=  speed*sin 

(CoG) ; 

vd_0 

=  Vd  sat; 

%  Accelerometer  Initial  Bias  Estimate 
ba_0  =  [ 0 ;  0 ;  0 ]  ; 

%  Initial  State  Vector 

x_bar2  =  [x_0;  y_0;  z_0;  vn_0;  ve_0;  vd_0;  ba_0] ; 

%  Initial  Covariance  Estimate 

Px2  =  diag ( [le-5*ones (1, 3) ,  le-5*ones (1, 3) ,  le-3*ones (1, 3) ] ) ; 
%  AHRS  SIGMA  Weights  Initialization 


nx  =  length (x  bar); 

q, 

o 

Number  of  AHRS  States 

nQ  =  length (diag (Qv) ) ; 

q, 

o 

Process  Noise  States 

nR  =  length (diag (R) ) ; 

g, 

o 

Measurement  Noise 

States 

nxa  =  nx+nQ+nR; 

g, 

o 

Total  Augmented 

States 

n  =  2*nxa+l; 

%  Number  of 

Iterations 

alpha  =  le-3; 

q, 

o 

Tunable  Scaling 

Factor 

beta  =  2; 

q, 

o 

beta  =  2  for  Gaussian 

PDF 

kappa  =  0 ; 

g, 

o 

Tunable  Secondary 

Scaling 

q, 

o 

Factor . 

lamda  =  alpha''2 *  (nxa+kappa) -nxa; 

q, 

o 

Weighting  Factor 

wm  =  (1/ (2* (nxa+lamda) ) ) *ones (n, 1) ; 

q, 

o 

Measurement  Weights 

wc  =  wm; 

g, 

o 

Covariance  Weights 

wm ( 1 )  =  lamda/ (nxa+lamda); 

q, 

o 

Zeroth  Measurement 

Weight 

wc  ( 1 )  =  wm  ( 1 )  +  ( l-alpha''2+beta)  ; 

q, 

o 

Zeroth  Covariance 

Weight 

eta  =  sqrt (nxa+lamda) ; 

g, 

o 

Covariance  Weighting 

q, 

o 

Factor 

%  INS  SIGMA  Weights  Initialization 

nx2  =  length (x  bar2); 

g, 

o 

Number  of  INS  States 

nQ2  =  length (diag (Qv2 )) ; 

q, 

o 

Process  Noise  States 

nR2  =  length (diag (R2 )) ; 

g, 

o 

Measurement  Noise 

States 

nxa2  =  nx2+nQ2+nR2; 

q, 

o 

Total  Augmented 

States 

n2  =  2*nxa2+l; 

%  Total  Iterations 
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alpha2  =  le-3; 
beta2  =  2; 

PDF 

k;appa2  =  0 ; 
Scaling 


%  Tunable,  0<alpha<l 
%  beta  =  2  for  Guassian 


o 


o 


Tunable  Secondary 


g, 

o 


Factor . 


Iamda2  =  alpha2 ^^2 *  (nxa2  +  k;appa2 ) -nxa2 ; 
wm2  =  (1/ (2* (nxa2+lamda2 ) ) ) *ones (n2, 1) ; 
wc2  =  wm2 ; 

wm2 ( 1 )  =  lamda2 / (nxa2+lamda2 ) ; 

Weight 

wc2(l)  =  wm2  ( 1 )  +  (l-alpha2''2+beta2)  ; 

Weight 

eta2  =  sqrt  (nxa2  +  lainda2  )  ; 


%  Weighting  Factor 


%  Measurement  Weights 
%  Covariance  Weights 
%  Zeroth  Measurement 


o 


Zeroth  Covariance 


0, 

o 


%  Covariance  Weighting 
Factor 


end 

%%  AHRS  ESTIMATOR 

%  Build  Augmented  State  Vector 

X  =  zeros (nxa,  n) ; 

xa  =  [x  bar;  zeros (nxa-nx, 1 )] ; 

%  Build  Augmented  Covariance  Matrix 
Pxa  =  [Px  zeros (nx,nQ)  zeros (nx, nR) ; 


zeros (nQ,nx)  Qv  zeros (nQ, nR) ; 
zeros (nR,nx)  zeros (nR,nQ)  R]  ; 


%  Build  2n+l  Sigma  Vectors 
c  =  eta*Pxa; 
for  k  =  1 : nxa+1 
if  k  ==  1 

X ( : , k)  =  xa; 

else 

X(:,k)  =  xa  +  c(k-l,:)'; 

X(:,k+nxa)  =  xa  -  c(k-l,:)'; 

end 

end 

%  Time  Update 
Xx  =  zeros (nx,  n) ; 

X  bar  =  zeros (nx,  1); 
for  k  =  l:n 

%  Gyro  Model  w  bi (true)  =  w_bi (measured)  -  bias  -  noise 
w_bi_b  =  [p-X ( 5 , k) -X ( 8 , k) ;  q-X (6, k) -X (9, k) ;  r-X(7,  k)-X(10,k)]; 

if  SIM  ==  1 

%  No  Sidereal  Rate 
w  bt  =  w  bi  b; 

else 

%  Sidereal  Rate  Included 
R_t2b  =  rot_t2b(X(l:4,k)); 

w  bt  =  w  bi  b  -  R  t2b* [wei*cos (latO) ;  0;  -wei*sin (latO) ] ; 
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end 


%  Quaternion  Update 

wl  =  w  bt(l);  w2  =  w  bt(2);  w3  =  w  bt(3); 

Q  kpl  =  X(l:4,  k)  +  (dt/2)*[0,  -wl,  -w2,  -w3; 


wl , 

0, 

w3. 

-w2 ; 

w2 , 

-w3. 

0, 

wl ; 

w3. 

w2 , 

-wl , 

0]*X(1:4,  k); 

%  Quaternion  Normalization 

Q  kpl  n  =  Q  kpl/sqrt(Q  kpl' 

* Q  kpl) 

r 

%  Gyro  Bias  Update 
if  SIM  ==  1 

%  Constant  Bias 
bg  =  X  ( 5  :  7 ,  k)  ; 

else 

%  Random  Walk  Bias 

bg  =  X(5:7,k)  +  dt*X(ll:13,k); 

end 


%  Rebuild  the  State  Vector  for  Each  Iteration 
Xx(:,k)  =  [Q_kpl_n;  bg] ; 


%  Calculated  the  Weighted  Mean  of  the  State  Vector 

X  bar  =  X  bar  +  wm ( k) *Xx ( : , k) ; 

end 

%  Calculate  Error  Covariance 
Px=  zeros (nx,  nx) ; 
for  k  =  l:n 

Px  =  Px  +  wc ( k) * ( (Xx ( : ,  k)  -  x  bar ) * (Xx ( : ,  k)  -  x  bar)'); 

end 


%  Non-Linear  Measurement  Process  Equations 
Y  =  zeros (nR,  n) ; 
y_bar  =  zeros (nR, 1 ) ; 

Heading  =  zeros (l,n); 
if  isempty(Hl) 

HI  =  zeros(l,n); 

H2  =  zeros ( 1 , n) ; 

Headings  =  zeros (l,n); 
ii  =  zeros ( 1 , n) ; 

end 

for  k  =  l:n 


%  Pre-fill 
%  Pre-fill 
%  Pre-fill 

%  Pre-fill 
%  Pre-fill 
%  Pre-fill 
%  Pre-fill 


%  Rotation  Matrix  (LTP  to  Body) 
R  t2b  =  rot  t2b (Xx ( 1 ; 4 , k) ) ; 

%  Gyro  Measurements 

w_bi  =  [p;  q;  r]  -  Xx(5:7,k); 

if  SIM  ==  1 

noise  R  =  X(ll:14,k); 
w  bt  =  w  bi; 

else 
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end 


noise  R  =  X ( 14 ; 17 , k) ; 

w  bt  =  w  bi  -  R  t2b* [wei*cos (latO) ;  0;  -wei*sin (latO) ] ; 


%  Quaternion  to  Euler  Angles 

qO  =  Xx(l,k);  ql  =  Xx(2,k);  q2  =  Xx(3,k);  q3  =  Xx(4,k); 

psi  =  mod  (atan2  (2*  (ql*q2+q0*q3)  ,  q0''2+ql''2-q2''2-q3''2)  +  noise  R(4), 

2*pi) ; 

%  Unwrap  Heading  Angle  From  [0,  2*pi]  Range  to  Aviod  Jumps 
if  nn  ==  1 

HI (k)  =  psi; 

HeadingO(k)  =  Hl(k); 

Heading (k)  =  Hl(k); 

else 

H2 (k)  =  psi; 

if  (H2 (k) -HI (k) )  <=  -100*pi/180 
ii(k)  =  ii(k)+l; 

end 

if  (H2 (k) -HI (k) )  >=  100*pi/180 
ii (k)  =  ii (k) -1; 

end 

Heading (k)  =  HeadingO (k) + ( (H2 (k) +2*pi*ii (k) )  -  HeadingO ( k) ) ; 

HI (k)  =  H2 (k) ; 

end 

%  Accelerometer  Estimate 

FB_hat  =  R_t2b* [ax_gps;  ay_gps;  az_gps]  +  ... 

cross (w_bt,  R_t2b*x_bar2 (4 : 6) )  -  ... 

R_t2b*[0;  0;  g] -x_bar2 ( 7 : 9 ) -noise_R ( 1 : 3 ) ; 

%  Calculated  Observations 
Y ( ; , k) = [FB_hat;  Heading (k)]; 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm ( k) *Y ( : , k) ; 
end 

%  Estimated  Observation  Covariance 
Py  =  zeros (nR, nR) ; 
for  k  =  1 : n 

Py  =  Py  +  wc ( k) * ( ( Y ( : , k)  -  y_bar) * (Y ( : , k)  -  y_bar) ' ) ; 

end 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx, nR) ; 
for  k  =  l:n 

Pxy  =  Pxy  +  wc ( k) * ( (Xx ( : , k)  -  x  bar) * (Y ( : , k)  -  y  bar)'); 

end 

%  Measurement  Method  2  (Accelerometer) 

%  Heading  Unwrap  from  [0,  2*pi] 
if  nn  ==  1 

H3  =  heading; 

H4  =  0; 
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psiO  =  H3; 
psi  m  =  H3; 

j  j=0; 

else 

H4  =  heading; 
if  (H4-H3)  <=  -100*pi/180 

jj  =  jj+1; 

end 

if  (H4-H3)  >=  100*pi/180 

jj  =  jj-1; 

end 

psi_m  =  psiO+ ( (H4+2*pi* j j )  -  psiO); 

H3  =  H4; 

end 

%  Measurement  Vector 
Z  =  [fx;  fy;  fz;  psi  m]  ; 

%  Kalman  Filter  Equations 
K  =  Pxy/Py; 

X  bar  =  X  bar  +  K* (Z  -  y  bar) ; 

%  Covariance  Corrections 
Px  =  Px  -  K*Py*K'; 

nn  =  2 ; 

%%  INS  ESTIMATOR 

%  Build  Augmented  State  Vector 
X  =  zeros (nxa2,  n2); 
xa  =  [x  bar2;  zeros (nxa2-nx2 , 1 )] ; 

%  Build  Augmented  Covariance  Matrix 
Pxa2  =  [Px2  zeros (nx2 , nQ2 )  zeros (nx2 , nR2 ) ; 

zeros (nQ2 , nx2 )  Qv2  zeros (nQ2 , nR2 ) ; 
zeros (nR2 , nx2 )  zeros (nR2 , nQ2 )  R2 ]  ; 

%  Construct  2n+l  SIGMA  Vectors 
c  =  eta2*Pxa2; 
for  k  =  l:nxa2+l 
if  k  ==  I 

X ( : , k)  =  xa; 

else 

X ( : , k)  =  xa  +  c(k-l,:)'; 
X(:,k+nxa2)  =  xa  -  c(k-l,:)'; 

end 

end 

%  Time  Update 

Xx  =  zeros (nx2,  n2); 

X  bar2  =  zeros (nx2,  1); 
for  k  =  1 : n2 
%  Rotation  Matrix 
R  t2b  =  rot  t2b(x  bar(l:4)); 


%  Pre-fill 
%  Pre-fill 


%  Kalman  Gain 
%  Kalman  Correction 
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R  b2t  =  R  t2b' ; 

%  Posittion  Update 

P_kpl  =  X(l:3,k)  +  dt*[X(4:5,k)  ;  -X(6,k)]; 

%  Velocity  and  Bias  Update 
if  SIM  ==  1 
%  No  Sidereal 

V_kpl  =  X(4:6,k)  +  dt* (R_b2t* ( [fx;  fy;  fz]  -  X(7:9,k)  -  ... 

X(10:12,  k))  +  [0;  0;  g] ) ; 
ba_kpl  =  X(7:9,k); 

else 

%  Sidereal  Included 

V  kpl  =  X(4:6,k)  +  dt* (R  b2t* ( [fx;  fy;  fz]  -  X(7:9,k)  -  ... 

X(10:12,  k) )  +  [0;  0;  g]  -  2 *cross ( [wei*cos ( latO ) ; 
0;  -wei*sin (latO) ]  ,  X(4:6,k))); 
ba_kpl  =  X(7:9,k)  +  dt*X (13 : 15, k) ; 

end 

%  States 

Xx ( : , k)  =  [P  kpl;  V  kpl;  ba  kpl]; 

%  Calculated  Mean 

X  bar2  =  x  bar2  +  wm2 ( k) *Xx ( : , k) ; 
end 

%  Predicted  Error  Covariance 
Px2=  zeros (nx2,  nx2); 
for  k  =  1 : n2 

Px2  =  Px2  +  wc2 ( k) * ( (Xx ( : , k)  -  x  bar2 ) * (Xx ( : , k)  -  x  bar2)') 

end 

%  Measurement  Equations 
Y  =  zeros (nR2,  n2); 
y_bar  =  zeros (nR2 , 1 ) ; 
for  k  =  1 : n2 

if  SIM  ==  1 

%  Calculated  Observations 
Y(:,k)=Xx(l:6,k)+X(13:18,k)  ; 

else 

%  Calculated  Observations 
Y(;,k)=Xx(l:6,k)+X(16:21,k)  ; 

end 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm2 ( k) *Y ( : , k) ; 
end 

%  Estimated  Observation  Covariance 
Py  =  zeros (nR2 , nR2 ) ; 
for  k  =  1 : n2 

Py  =  Py  +  wc2 ( k) * ( ( Y ( : , k)  -  y_bar) * (Y ( : , k)  -  y_bar) ' ) ; 

end 
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%  Pre-fill 
%  Pre-fill 


%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx2  ,  nR2 )  ; 
for  k  =  1 : n2 

Pxy  =  Pxy  +  wc2 ( k) * ( (Xx ( : , k)  -  x  bar2 ) * ( Y ( : , k)  -  y  bar)'); 

end 

%  Measurements 

Z  =  [N_sat;  E_sat;  D_sat;  speed*cos (CoG) ;  speed*sin (CoG)  ;  Vd_sat] ; 

%  Kalman  Filter  Equations 

K  =  Pxy/Py;  %  Kalman  Gain 

%  Kalman  State  Corrections 
X  bar2  =  x  bar2  +  K* (Z  -  y  bar) ; 

%  Covariance  Correction 
Px2  =  Px2  -  K*Py*K'; 

X  HAT  =  [x  bar (1:4);  x  bar2(l:6);  x  bar (5: 7);  x  bar2(7:9)]; 
end 

kk=kk+l; 

end 

function  R  t2b  =  rot  t2b(x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

R_t2b  =  [q0^2+ql^2-q2^2-q3^2  2* (ql*q2+q0*q3)  2*(ql*q3-q0*q2); 

2*  (q2*ql-q0*q3)  qO ^^2 -ql ''2+q2 ^2 -q3 ''2  2*(q2*q3+q0*ql); 

2*  (q3*ql  +  q0*q2)  2  *  ( q3  *q2 -qO  *ql )  qO  ^^2 -ql ''2 -q2 ''2  +  q3 ''2  ]  ; 

end 

function  q  =  e2q(phi,  theta,  psi) 

%  Form  Rotation  Matrix 

R_psi  =  [cos (psi)  sin (psi)  0;  -sin (psi)  cos (psi)  0;  0  0  1]; 

R_theta  =  [cos (theta)  0  -sin  (theta);  0  1  0;  sin  (theta)  0  cos (theta) ] 
R_phi  =[10  0;  0  cos (phi)  sin (phi);  0  -sin (phi)  cos  (phi)]; 

R  n2b  =  R  phi*R  theta*R  psi; 

%  Extract  Quaternions 
q0=sqrt (1+trace (R_n2b) ) /2; 
ql= (R_n2b (2,3) -R_n2b (3, 2) ) / (4*q0) ; 
q2= (R_n2b (3,1) -R_n2b (1,3) ) / ( 4 *q0 )  ; 
q3= (R_n2b (1, 2) -R_n2b (2,1))/ (4*q0)  ; 

q  =  [qO;  ql;  q2 ;  q3]; 
end 

D.  SQUARE  ROOT  UNSCENTED  KALMAN  FILTER  MATLAB  CODE 
1,  Measurement  Model  One  Implementation 

function  X_HAT  =  SRUKFl (u) 

%%  SQUARE  ROOOT  UNSCENTED  KALMAN  FILTER 
%  Measurement  Model  1 
%  LT  Steven  Terjesen 
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%  This  estimator  is  built  for  use  in  MATLAB  Simulink.  There  are  38 
inputs 

%  required  and  can  be  run  in  Simulink  with  an  "Interpolated  MATLAB 
%  Function"  block.  The  first  13  inputs  are  the  vehicle  data:  Course 
Over 

%  Ground,  Speed  Over  Ground,  Accelerometer  Measurements,  Gyro 
Measurements , 

%  GPS  Measurements  (In  LTP  XNorth-YEast-ZDown) ,  Heading  Measurement, 
and 

%  Vertical  Velocity  (zeroed  out  for  SEAFOXII  data) .  Inputs  14  through 
34 

%  are  the  Process  Noise  and  Measurement  Noise  diagonal  elements.  These 
%  elements  are  not  hard  coded  to  allow  for  easier  tuning.  Inputs  35 
%  through  37  are  the  N-E-D  accelerations  in  the  LTP  frame.  Input  38  is 
a 

%  toggle  for  selecting  whether  the  data  is  from  Condor  or  SEAFOX  II. 

%%  System  Inputs 
%  Measurement  inputs 


CoG  =  u ( 1 ) ; 

%GPS 

Course  Over  Ground 

speed  =  u(2); 

%GPS 

Speed  Over  Ground 

fx  =  u (3) ;  fy 

=  u(4) 

;  f  z  =  u  ( 5 )  ; 

%IMU 

Accelerations 

p  =  u  ( 6 )  ;  q  = 

u(7)  ; 

r  =  u  ( 8 )  ; 

%IMU 

Angular  Rates 

N  sat  =  u ( 9 ) ; 

E  sat 

=  u(10);  D  sat  =  u(ll); 

%GPS 

Position  (NED) 

heading  =  u ( 12 ) ; 

%GPS 

Heading 

Vd  sat  =  u(13) 

'  ; 

%GPS 

LTP  Down  Velocity 

%  Process  and  Measurement  Noise 
R  =  diag (u (14 : 16) ) ; 

R2  =  diag (u ( 17 : 22 ) ) ; 

SIM  =  u  (38) ; 

SEAFOXII (2) 

q, 

o 

if  SIM  ==  1 

Qv  =  diag (u (23 : 25) )  ; 

Condor 

Qv2  =  diag (u (29 : 31) ) ; 

Condor 

else 

Qv  =  diag (u (23 : 2 8 ) ) ; 

SEAFOX 

Qv2  =  diag (u (2  9 : 34) )  ; 

SEAFOX 

Vd_sat  =  0; 

q, 

o 

on 

q, 

o 

D_sat  =  0; 

Measurement 


Matrices 

%AHRS  Measurement  Noise 
%INS  Measurement  Noise 

%Condor(l)  or 
Simulation  Selector 

%AHRS  Process  Noise 
%INS  Process  Noise 

%AHRS  Process  Noise 

%INS  Process  Noise 

%No  Vertical  Velocity 
measurement  available 

SEAFOX  II,  set  Vd=0. 
%No  Altitude 
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set 


available  on  SEAFOXII, 


0, 

o 

end 


D  sat  =  0; 


%  GPS  Accelerations 

ax_gps  =  u(35);  ay_gps  =  u(36);  az_gps  =  u(37);  %GPS  Accelerations  as 

%  calculated  from  3rd 
%  Order  Filter 


%%  Initialization 

persistent  x  bar  HeadingO  HI  H2  ii  j j  psiO  H3  H4  ... 

Sx  Sx2  kk  dt  g  nn  nx  nxa  nQ  nR  n  wc  wm  x  bar2  nx2  nxa2  n2  nQ2  nR2  wc2 
wm2  wei  latO  eta  eta2 

%  Allows  time  for  Condor  To  steady  out  during  live  testing 
if  isempty(kk) 
kk  =  0; 

end 

if  kk  <  1  &&  SIM==1 

X_HAT  =  zeros (16,1); 

else 

if  isempty(x  bar) 

%  Miscellaneous 
dt  =  0.01; 
g  =  9.8; 

[m/s^2 ] 

wei  =  7.292115*10^-5; 
lat0=  0.639268394832413; 

%lon0  =  -2.115435878466264 
nn=l ; 

Initializer 

%  AHRS  Initialization 
%  Euler  Angle  Initialization 

rx  =  ax  gps*cos (heading) +ay  gps*sin (heading) ; 
ry  =  -ax  gps*sin (heading) +ay  gps*cos (heading) ; 
rz  =  az_gps-g; 

theta_0  =  atan((-rx*rz  -  fx*sqrt  (rx^2+rz''2-fx^2)  )  /  (fx^2-rz''2)  )  ; 
r_theta  =  rx*sin (theta_0)  +  rz*cos (theta_0) ; 
fc  =  f y- speed* r; 

phi  0  =  atan((r  theta*ry  +  fc*sqrt  (ry''2+r  theta''2-fc''2)  )  /  (fc''2- 
r_theta^2 ) )  ; 
psi  0=heading; 

%  Euler  Angles  to  Quaternions 
q_0  =  e2q(phi_0,  theta_0,  psi_0); 

%  Gyro  Bias  Initial  Values 
bg_0  =  [ 0 ;  0 ;  0 ] ; 

%  Inital  State  Vector 
x_bar  =  [q_0;  bg_0] ; 

%  Initial  Covariance  Estimate 
Px  =  diag ( [le-5*ones (1, 4)  le-3*ones (1, 3) ] )  ; 


%Filter  dt  [sec] 
%Gravity  Constant 

%Sidereal  Rate  [rad/s] 
%Origin  of  LTP  [rad] 

%Heading  Count 
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%  Inital  Matrix  Square  Root 
Sx  =  chol ( Px) ; 


%INS  Initialization 

%  Position  &  Velocity  Initialization 


II 

o 

1 

X 

N  sat; 

y_0  = 

E  sat; 

z  0  = 

D  sat; 

vn  0 

=  speed*cos 

(CoG) ; 

ve  0 

=  speed*sin 

(CoG) ; 

vd_0 

=  Vd  sat; 

%  Accelerometer  Initial  Bias  Estimate 
ba_0  =  [ 0 ;  0 ;  0 ]  ; 

%  Initial  State  Vector 

x_bar2  =  [x_0;  y_0;  z_0;  vn_0;  ve_0;  vd_0;  ba_0] ; 

%  Initial  Covariance  Estimate 

Px2  =  diag ( [le-5*ones (1, 3) ,  le-5*ones (1, 3) ,  le-3*ones (1, 3) ] ) ; 
%  Inital  Matrix  Square  Root 
Sx2  =  chol(Px2); 


%  AHRS  SIGMA  Weights  Initialization 
nx  =  length (x  bar); 
nQ  =  length (diag (Qv) ) ; 
nR  =  length (diag (R) ) ; 

States 

nxa  =  nx+nQ+nR; 

States 

n  =  2*nxa+l; 

Iterations 
alpha  =  le-3; 

Factor 
beta  =  2; 

PDF 

kappa  =  0 ; 

Scaling 

g, 

o 

lamda  =  alpha^2* (nxa+kappa) -nxa; 
wm  =  (1/ (2* (nxa+lamda) ) ) *ones (n, 1) ; 
wc  =  wm; 

wm ( 1 )  =  lamda/ (nxa+lamda); 

Weight 

wc ( 1 )  =  wm ( 1 )  +  ( l-alpha^2+beta) ; 
Weight 

eta  =  sqrt (nxa+lamda) ; 

g, 

o 


%  Number  of  AHRS  States 
%  Process  Noise  States 
%  Measurement  Noise 

%  Total  Augmented 

%  Number  of 

%  Tunable  Scaling 

%  beta  =  2  for  Gaussian 

%  Tunable  Secondary 

Factor . 

%  Weighting  Factor 
%  Measurement  Weights 
%  Covariance  Weights 
%  Zeroth  Measurement 

%  Zeroth  Covariance 

%  Covariance  Weighting 
Factor 


%  INS  SIGMA  Weights  Initialization 
nx2  =  length (x  bar2); 
nQ2  =  length (diag (Qv2 )) ; 
nR2  =  length (diag (R2 )) ; 

States 

nxa2  =  nx2+nQ2+nR2; 

States 

n2  =  2*nxa2+l; 
alpha2  =  le-3; 
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%  Number  of  INS  States 
%  Process  Noise  States 
%  Measurement  Noise 

%  Total  Augmented 

%  Total  Iterations 
%  Tunable,  0<alpha<l 


beta2  =  2; 
PDF 

k;appa2  =  0 ; 
Scaling 


%  beta  =  2  for  Gaussian 


o 


Tunable  Secondary 


0, 

o 


Factor . 


Iamda2  =  alpha2^2*  (nxa2  +  k;appa2) -nxa2; 
wm2  =  (1/ (2* (nxa2+lamda2 ) ) ) *ones (n2, 1) ; 
wc2  =  wm2 ; 

wm2  ( 1 )  =  lainda2  /  (nxa2  +  lamda2  )  ; 

Weight 

wc2(l)  =  wm2 ( 1 )  +  (l-alpha2^2+beta2) ; 

Weight 

eta2  =  sqrt (nxa2+lamda2 ) ; 


%  Weighting  Factor 


%  Measurement  Weights 
%  Covariance  Weights 
%  Zeroth  Measurement 


o 


o 


Zeroth  Covariance 


q, 

o 


%  Covariance  Weighting 
Factor 


end 

%%  AHRS  ESTIMATOR 

%  Build  Augmented  State  Vector 

X  =  zeros (nxa,  n) ; 

xa  =  [x  bar;  zeros (nxa-nx, 1 )] ; 

%  Build  Augmented  Covariance  Matrix 
Sxa  =  [Sx  zeros (nx,nQ)  zeros (nx, nR) ; 


zeros (nQ,nx)  Qv  zeros (nQ, nR) ; 
zeros (nR,nx)  zeros (nR,nQ)  R]  ; 


%  Build  2n+l  Sigma  Vectors 
c  =  eta*Sxa; 
for  k  =  1 : nxa+1 
if  k  ==  1 

X ( : , k)  =  xa; 

else 

X(:,k)  =  xa  +  c(k-l,:)'; 

X(:,k+nxa)  =  xa  -  c(k-l,:)'; 

end 

end 

%  Time  Update 
Xx  =  zeros (nx,  n) ; 

X  bar  =  zeros (nx,  1); 
for  k  =  1 : n 

%  Gyro  Model  w_bi (true)  =  w_bi (measured)  -  bias  -  noise 
w_bi_b  =  [p-X ( 5 , k) -X ( 8 , k) ;  q-X (6, k) -X (9, k) ;  r-X(7,  k)-X(10,k)]; 

if  SIM  ==  1 

%  No  Sidereal  Rate 
w  bt  =  w  bi  b; 

else 

%  Sidereal  Rate  Included 
R_t2b  =  rot_t2b(X(l:4,k)); 

w  bt  =  w  bi  b  -  R  t2b* [wei*cos (latO) ;  0;  -wei*sin (latO) ] ; 


end 
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%  Quaternion  Update 

wl  =  w  bt(l);  w2  =  w  bt(2);  w3  =  w  bt(3); 

Q  kpl  =  X(l:4,  k)  +  (dt/2)*[0,  -wl,  -w2,  -w3; 

wl,  0,  w3,  -w2; 

w2 ,  -w3,  0,  wl; 

w3,  w2 ,  -wl,  0]*X(1:4,  k) ; 

%  Quaternion  Normalization 
Q  kpl  n  =  Q  kpl/sqrt(Q  kpl'*Q  kpl); 

%  Gyro  Bias  Update 
if  SIM  ==  1 

%  Constant  Bias 
bg  =  X  ( 5  :  7 ,  k)  ; 

else 

%  Random  Walk  Bias 
bg  =  X(5:7,k)  +  dt*X(ll:13,k); 

end 

%  Rebuild  the  State  Vector  for  Each  Iteration 
Xx(:,k)  =  [Q_kpl_n;  bg] ; 

%  Calculated  the  Weighted  Mean  of  the  State  Vector 
X  bar  =  X  bar  +  wm ( k) *Xx ( : , k) ; 
end 

%  Calculate  Error  Covariance 

Ax  =  sqrt (wc (2 ) ) * (Xx ( : , 2 : n)  -  x_bar ( : , ones ( 1 , n-1 ) ) ) ; 

%  QR  Decomposition 
[~,  sx]  =  qr  (Ax'  ,  0)  ; 

%  Cholesky  Update  (Downdate  for  negative  zeroth  weight) 

Sx  =  cholupdate ( sx,  sqrt ( -wc ( 1 ) ) * (Xx ( : , 1 ) - 

%  Non-Linear  Measurement  Process  Equations 
Y  =  zeros (nR,  n) ; 
y_bar  =  zeros (nR, 1); 

Heading  =  zeros (l,n); 
if  isempty(Hl) 

HI  =  zeros ( 1 , n) ; 

H2  =  zeros ( 1 , n) ; 

Headings  =  zeros (l,n); 
ii  =  zeros ( 1 , n) ; 

end 

for  k  =  1 : n 

if  SIM  ==  1 

noise  R  =  X(ll:13,k); 

else 

noise  R  =  X(14:16,k); 

end 

%  Quaternion  to  Euler  Angles 
qO  =  Xx(l,k);  ql  =  Xx(2,k);  q2  =  Xx(3,k);  q3  =  Xx(4,k); 
phi  =  atan2  (2*  (q2*q3+q0*ql)  ,  q0''2-ql''2-q2''2+q3''2)  +  noise  R(l) 
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X  bar) , 


g, 

o 

q, 

o 

g, 

o 


g, 

o 

g, 

o 

q, 

o 

g, 

o 


)  ; 


Pre-fill 

Pre-fill 

Pre-fill 

Pre-fill 

Pre-fill 

Pre-fill 

Pre-fill 


theta  =  asin (-2* (ql*q3-q0*q2) ) +  noise_R(2); 

psi  =  mod  (atan2  (2*  (ql*q2+q0*q3)  ,  q0''2+ql''2-q2''2-q3''2)  +  noise  R(3), 
2*pi) ; 

%  Unwrap  Heading  Angle  From  [0,  2*pi]  Range  to  Avoid  Jumps 
if  nn  ==  1 

HI (k)  =  psi; 

HeadingO(k)  =  Hl(k); 

Heading (k)  =  Hl(k); 

else 

H2 (k)  =  psi; 

if  (H2 (k) -HI (k) )  <=  -100*pi/180 
ii(k)  =  ii(k)+l; 

end 

if  (H2 (k) -HI (k) )  >=  100*pi/180 
ii (k)  =  ii (k) -1; 

end 

Heading (k)  =  HeadingO (k) + ( (H2 (k) +2*pi*ii (k) )  -  HeadingO ( k) ) ; 

HI (k)  =  H2 (k) ; 

end 

%  Calculated  Observations 
Y(:,k)=[phi;  theta;  Heading ( k)]; 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm ( k) *Y ( ; , k) ; 
end 

%  Estimated  Observation  Covariance 

Bx  =  sqrt (wc (2 ) ) * (Y ( :  ,  2 : n)  -  y_bar ( : , ones ( 1 , n-1 ) ) ) ; 

%  QR  Decomposition 

[~,  sy]  =  qr(Bx',0);  %  **"qr"  PRODUCES  UPPER  TRIANGULAR  MATRIX** 

%  Cholesky  Update  (or  Downdate) 

Sy  =  cholupdate ( sy,  sqrt ( -wc ( 1 ) ) * ( Y ( : , I ) -y_bar ) ,  ; 

%  ***Sy  MUST  BE  LOWER  TRIANGULAR*** 

Sy  =  Sy'  ; 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx, nR) ; 
for  k  =  1 : n 

Pxy  =  Pxy  +  wc ( k) * ( (Xx ( : , k)  -  x_bar) * (Y ( : , k)  -  y_bar) ' ) ; 

end 

%  Measurement  Method  1  (Accelerometer-GPS-Compass  Fusion) 

%  Rotate  GPS  XYZ  Acceleration  about  Heading 
rx  =  ax  gps*cos (heading) +ay  gps*sin (heading) ; 
ry  =  -ax_gps*sin (heading) +ay_gps*cos (heading) ; 
rz  =  az_gps-g; 

%  Computer  Pitch 

theta  =  atan((-rx*rz  -  fx*sqrt (rx^2+rz^2-fx^2 ) )  /  (fx^2-rz^2) ) ; 

%  Compute  Roll 

r_theta  =  rx*sin (theta)  +  rz*cos (theta) ; 

fc  =  fy-(norm(x  bar2 (4 : 6) ) ) *r;  %  Added  Coriolis  Term 
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phi  =  atan((r  theta*ry  +  fc*sqrt (ry^2+r  theta^ 
r_theta''2 )  )  ; 

%  Heading  Unwrap  from  [0,  2*pi] 
if  nn  ==  1 

H3  =  heading; 

H4  =  0; 
psiO  =  H3; 
psi  m  =  H3; 

j  j=0; 

else 

H4  =  heading; 

if  (H4-H3)  <=  -100*pi/180 

jj  =  jj+1; 

end 

if  (H4-H3)  >=  100*pi/180 

jj  =  jj-1; 

end 

psi_m  =  psi0+ ( (H4+2*pi* j j )  -  psiO); 

H3  =  H4; 

end 

%  Measurement  Vector 
Z  =  [phi;  theta;  psi_m] ; 

%  Kalman  Filter  Equations 
K  =  (Pxy/Sy' ) /Sy; 

Squares 

X  bar  =  X  bar  +  K* (Z  -  y  bar)  ; 

%  Covariance  Corrections 
Ux  =  K*Sy; 
for  k  =  1 : nR 

Sx  =  cholupdate ( Sx,  Ux ( : ,  k)  , 

end 

Sx  =  Sx' ; 

TRIANGULAR 

nn  =  2 ; 

%%  INS  ESTIMATOR 

%  Build  Augmented  State  Vector 

X  =  zeros (nxa2,  n2); 

xa  =  [x  bar2;  zeros (nxa2-nx2 , 1 )] ; 

%  Build  Augmented  Covariance  Matrix 

Sxa2  =  [Sx2  zeros (nx2 , nQ2 )  zeros (nx2 , nR2 ) ; 

zeros (nQ2 , nx2 )  Qv2  zeros (nQ2 , nR2 ) ; 
zeros (nR2 , nx2 )  zeros (nR2 , nQ2 )  R2 ]  ; 

%  Construct  n+2  SIGMA  Vectors 
c  =  eta2*Sxa2; 
for  k  =  1 : nxa2+I 
if  k  ==  1 


-fc"2))  /  (fc"2- 


%  Efficient  Least 
%  Kalman  Correction 

%  Cholesky  Downdate 
%  MUST  BE  LOWER 
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X ( : , k)  =  xa; 

else 

X(:,k)  =  xa  +  c(k-l,:)'; 

X(:,k+nxa2)  =  xa  -  c(k-l,:)'; 

end 

end 

%  Time  Update 

Xx  =  zeros (nx2,  n2); 

X  bar2  =  zeros (nx2,  1); 

for  k  =  1 : n2 

%  Rotation  Matrix 

R_t2b  =  rot_t2b (x_bar ( 1 : 4 ) ) ; 

R  b2t  =  R  t2b' ; 

%  Position  Update 

P_kpl  =  X(l:3,k)  +  dt*[X(4:5,k)  ;  -X(6,k)]; 

%  Velocity  and  Bias  Update 
if  SIM  ==  1 
%  No  Sidereal 

V  kpl  =  X(4:6,k)  +  dt* (R  b2t* ( [fx;  fy;  fz]  -  X(7 

X(10:12,  k) )  +  [0;  0;  g] ) ; 
ba_kpl  =  X(7:9,k); 

else 

%  Sidereal  Included 

V  kpl  =  X(4:6,k)  +  dt* (R  b2t* ( [fx;  fy;  fz]  -  X(7 

X(10:12,  k) )  +  [0;  0;  g]  -  2*cross ( [wei* 
0;  -wei*sin (latO) ] ,  X(4:6,k))); 
ba_kpl  =  X(7:9,k)  +  dt*X(13:15,k); 

end 

%  States 

Xx ( : , k)  =  [P  kpl;  V  kpl;  ba  kpl]; 

%  Calculated  Mean 

X  bar2  =  x  bar2  +  wm2 ( k) *Xx ( : , k) ; 
end 

%  Predicted  Error  Covariance 

Ax  =  sqrt (wc2 (2 ) ) * (Xx ( : , 2 : n2 )  -  x  bar2 ( : , ones ( 1 , n2-l 

%  QR  Decomposition 
[~,  Sx2]  =  qr  (Ax'  ,  0)  ; 

%  Cholesky  Factor  Update 

Sx2  =  cholupdate ( Sx2 ,  sqrt ( -wc2 ( 1 ) ) * (Xx ( : , 1 ) -x_bar2 ) 

%  Measurement  Equations 
Y  =  zeros (nR2,  n2); 
y_bar  =  zeros (nR2 , 1 ) ; 
for  k  =  1 : n2 

if  SIM  ==  1 

%  Calculated  Observations 
Y(:,k)=Xx(l:6,k)+X(13:18,k)  ; 


%  Pre-fill 
%  Pre-fill 


:9,k)  - 


:9,k)  -  .  . 

cos ( latO ) ; 


%  Pre-fill 
%  Pre-fill 
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else 


%  Calculated  Observations 
Y  ( : ,  k)  =Xx  (1 :  6,  k)  +X  ( 1 6  :  2 1 ,  k)  ; 

end 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm2 ( k) *Y ( : , k) ; 
end 

%  Estimated  Observation  Covariance 

Bx  =  sqrt (wc2 (2 ) ) * (Y ( :  ,  2 : n2 )  -  y_bar ( : , ones ( 1 , n2-l ) ) ) ; 

%  QR  Decomposition 
[~,  Sy]  =  qr (Bx' , 0) ; 

%  Cholesky  Factor  Update 

Sy  =  cholupdate ( Sy,  sqrt ( -wc2 ( 1 ) ) * ( Y ( : , 1 ) -y_bar ) , 

%  Make  Sy  Lower  Triangular 
Sy  =  Sy' ; 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx2 , nR2 ) ; 
for  k  =  1 : n2 

Pxy  =  Pxy  +  wc2 ( k) * ( (Xx ( : , k)  -  x  bar2 ) * ( Y ( : , k)  -  y  bar)'); 

end 

%  Measurements 

Z  =  [N_sat;  E_sat;  D_sat;  speed*cos (CoG) ;  speed*sin (CoG) ;  Vd_sat] 

%  Kalman  Filter  Equations 
%  Kalman  Gain  w/  Efficient  Least  Squares 
K  =  (Pxy/Sy' ) /Sy; 

%  Kalman  State  Corrections 
X  bar2  =  x  bar2  +  K* (Z  -  y  bar)  ; 

%  Covariance  Correction 
Ux  =  K*Sy; 
for  k  =  1 : nR2 

Sx2  =  cholupdate ( Sx2 ,  Ux ( : , k) ,  '-' ) ; 

end 

%  Make  Sx2  Lower  Right  Triangular 
Sx2  =  Sx2 ' ; 

X  HAT  =  [x  bar (1:4);  x  bar2(l:6);  x  bar (5: 7);  x  bar2(7:9)]; 
end 

kk=kk+l; 

end 

function  R  t2b  =  rot  t2b(x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

R_t2b  =  [q0''2  +  ql''2-q2''2-q3''2  2*  (ql*q2  +  q0*q3)  2*(ql*q3-q0*q2); 

2* (q2*ql-q0*q3)  qO ^2 -ql ^2+q2 ^2 -q3 ^2  2*(q2*q3+q0*ql); 

2*  (q3*ql  +  q0*q2)  2  *  ( q3  *q2 -qO  *ql )  qO  ^^2 -ql ''2 -q2  ^2  +  q3 ''2  ]  ; 

end 

function  q  =  e2q(phi,  theta,  psi) 

%  Form  Rotation  Matrix 
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R_psi  =  [cos(psi)  sin(psi)  0;  -sin(psi)  cos(psi)  0;  0  0  1]; 

R_theta  =  [cos (theta)  0  -sin (theta) ;  0  1  0;  sin (theta)  0  cos (theta) ]  ; 
R_phi  =[10  0;  0  cos (phi)  sin (phi);  0  -sin (phi)  cos (phi) ]; 

R  n2b  =  R  phi*R  theta*R  psi; 

%  Extract  Quaternions 
q0=sqrt (1+trace (R_n2b) ) /2; 
ql= (R_n2b (2,3) -R_n2b (3, 2) ) / (4*q0) ; 
q2= (R_n2b (3,1) -R_n2b (1,3) ) / ( 4 *q0 )  ; 
q3= (R_n2b (1, 2) -R_n2b (2,1))/ (4*q0)  ; 

q  =  [qO;  ql;  q2 ;  q3]; 
end 


2,  Measurement  Model  Two  Implementation 

function  X_HAT  =  SRUKF2 (u) 

%%  SQUARE  ROOOT  UNSCENTED  KALMAN  FILTER 
%  Measurement  Model  2 
%  LT  Steven  Terjesen 
%  September  2014 

%  This  estimator  is  built  for  use  in  MATLAB  Simulink.  There  are  39 
inputs 

%  required  and  can  be  run  in  Simulink  with  an  "Interpolated  MATLAB 
%  Function"  block.  The  first  13  inputs  are  the  vehicle  data:  Course 
Over 

%  Ground,  Speed  Over  Ground,  Accelerometer  Measurements,  Gyro 
Measurements , 

%  GPS  Measurements  (In  LTP  XNorth-YEast-ZDown) ,  Heading  Measurement, 
and 

%  Vertical  Velocity  (zeroed  out  for  SEAFOXII  data) .  Inputs  14  through 
35 

%  are  the  Process  Noise  and  Measurement  Noise  diagonal  elements.  These 
%  elements  are  not  hard  coded  to  allow  for  easier  tuning.  Inputs  36 
%  through  38  are  the  N-E-D  accelerations  in  the  LTP  frame.  Input  39  is 
a 

%  toggle  for  selecting  whether  the  data  is  from  Condor  or  SEAFOX  II. 

%%  System  Inputs 
%  Measurement  inputs 


CoG  =  u ( 1 ) ; 

%GPS 

Course  Over  Ground 

speed  =  u(2); 

%GPS 

Speed  Over  Ground 

fx  =  u  ( 3 )  ;  f  y 

=  u(4) 

;  f  z  =  u  ( 5 )  ; 

%IMU 

Accelerations 

p  =  u  (  6 )  ;  q  = 

u(7)  ; 

r  =  u  ( 8 )  ; 

%IMU 

Angular  Rates 

N  sat  =  u ( 9) ; 

E  sat 

=  u(10);  D  sat  =  u(ll); 

%GPS 

Position  (NED) 

heading  =  u ( 12 ) ; 

%GPS 

Heading 

Vd  sat  =  u(13) 

'  ; 

%GPS 

LTP  Down  Velocity 

%  Process  and  Measurement  Noise  Matrices 

R  =  chol (diag (u ( 14 : 17 ) ) ) ;  %AHRS  Measurement  Noise 

R2  =  chol (diag (u ( 1 8 : 23 )  )  )  ;  %INS  Measurement  Noise 
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Condor (1)  or 


SIM  =  u (39) ; 

SEAFOXII (2) 

%  Simulation  Selector 


if  SIM  ==  1 

Qv  =  chol (diag (u (24 : 26) ) ) ; 
Condor 

Qv2  =  chol (diag (u (30 : 32) ) ) ; 
Condor 
else 

Qv  =  chol (diag (u (24 : 29) ) ) ; 
SEAFOX 

Qv2  =  chol (diag (u (30 : 35) ) ) ; 
SEAFOX 

Vd_sat  =  0; 

q, 

o 

on 

q, 

o 

D_sat  =  0; 

Measurement 

q, 

o 

set 

q, 

o 

end 


%AHRS  Process  Noise 
%INS  Process  Noise 

%AHRS  Process  Noise 

%INS  Process  Noise 

%No  Vertical  Velocity 
measurement  available 

SEAFOX  II,  set  Vd=0. 
%No  Altitude 

available  on  SEAFOXII, 

D  sat  =  0; 


%  GPS  Accelerations 

ax_gps  =  u(36);  ay_gps  =  u(37);  az_gps  =  u(38);  %GPS  Accelerations  as 

%  calculated  from  3rd 
%  Order  Filter 


%%  Initialization 

persistent  x  bar  HeadingO  HI  H2  ii  j j  psiO  H3  H4  ... 

Sx  Sx2  kk  dt  g  nn  nx  nxa  nQ  nR  n  wc  wm  x  bar2  nx2  nxa2  n2  nQ2  nR2  wc2 
wm2  wei  latO  eta  eta2 

%  Allows  time  for  Condor  To  steady  out  during  live  testing 
if  isempty(kk) 
kk  =  0; 

end 

if  kk  <  1  &&  SIM==1 

X_HAT  =  zeros (16,1); 

else 

if  isempty(x  bar) 

%  Miscellaneous 
dt  =  0.01; 
g  =  9.8; 

[m/s''2  ] 

wei  =  7.292115*10^-5; 
lat0=  0.639268394832413; 

%lon0  =  -2.115435878466264 
nn=l ; 

Initializer 


%Filter  dt  [sec] 
%Gravity  Constant 

%Sidereal  Rate  [rad/s] 
%Origin  of  LTP  [rad] 

%Heading  Count 
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%  AHRS  Initialization 
%  Euler  Angle  Initialization 

rx  =  ax  gps*cos (heading) +ay  gps*sin (heading) ; 
ry  =  -ax_gps*sin (heading) +ay_gps*cos (heading)  ; 
rz  =  az_gps-g; 

theta  0  =  atan((-rx*rz  -  fx*sqrt  (rx''2+rz''2-fx''2)  )  /  (fx^2-rz''2)  )  ; 
r_theta  =  rx*sin (theta_0)  +  rz*cos (theta_0) ; 
fc  =  f y- speed* r; 

phi  0  =  atan((r  theta*ry  +  fc*sqrt (ry^2+r  theta^2-fc^2) ) / (fc^2- 
r_theta^2 ) ) ; 
psi  0=heading; 

%  Euler  Angles  to  Quaternions 
q_0  =  e2q(phi_0,  theta_0,  psi_0); 

%  Gyro  Bias  Initial  Values 
bg_0  =  [ 0 ;  0 ;  0 ] ; 

%  Inital  State  Vector 
x_bar  =  [q_0;  bg_0] ; 

%  Initial  Covariance  Estimate 

Px  =  diag ( [ le-5*ones ( 1 , 4 )  le-3*ones (1, 3) ] )  ; 

%  Inital  Matrix  Square  Root 
Sx  =  chol ( Px)  ; 

%INS  Initialization 

%  Position  &  Velocity  Initialization 


II 

o 

1 

X 

N  sat; 

y_0  = 

E  sat; 

z  0  = 

D  sat; 

vn  0 

=  speed*cos 

(CoG) ; 

ve  0 

=  speed*sin 

(CoG) ; 

vd_0 

=  Vd  sat; 

%  Accelerometer  Initial  Bias  Estimate 
ba_0  =  [ 0 ;  0 ;  0 ]  ; 

%  Initial  State  Vector 

x_bar2  =  [x_0;  y_0;  z_0;  vn_0;  ve_0;  vd_0;  ba_0] ; 

%  Initial  Covariance  Estimate 

Px2  =  diag ( [le-5*ones (1, 3)  ,  le-5*ones (1, 3)  ,  le-3*ones (1, 3) ] ) ; 
%  Inital  Matrix  Square  Root 
Sx2  =  chol(Px2); 


%  AHRS  SIGMA  Weights  Initialization 
nx  =  length (x  bar); 
nQ  =  length (diag (Qv) ) ; 
nR  =  length (diag (R) ) ; 

States 

nxa  =  nx+nQ+nR; 

States 

n  =  2*nxa+l; 

Iterations 
alpha  =  le-3; 

Factor 
beta  =  2; 

PDF 

kappa  =  0 ; 

Scaling 


%  Number  of  AHRS  States 
%  Process  Noise  States 
%  Measurement  Noise 

%  Total  Augmented 

%  Number  of 

%  Tunable  Scaling 

%  beta  =  2  for  Gaussian 

%  Tunable  Secondary 
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g, 

o 

lamda  =  alpha''2 *  (nxa+kappa) -nxa; 
wm  =  (1/ (2* (nxa+lamda) ) ) *ones (n, 1) ; 
wc  =  wm; 

wm ( 1 )  =  lamda/ (nxa+lamda); 

Weight 

wc  ( 1 )  =  wm  ( 1 )  +  ( l-alpha''2+beta)  ; 
Weight 

eta  =  sqrt (nxa+lamda) ; 

0, 

o 


Factor . 

%  Weighting  Factor 
%  Measurement  Weights 
%  Covariance  Weights 
%  Zeroth  Measurement 

%  Zeroth  Covariance 

%  Covariance  Weighting 
Factor 


%  INS  SIGMA  Weights  Initialization 
nx2  =  length (x  bar2); 
nQ2  =  length (diag (Qv2 )) ; 
nR2  =  length (diag (R2 )) ; 

States 

nxa2  =  nx2+nQ2+nR2; 

States 

n2  =  2*nxa2+l; 
alpha2  =  le-3; 
beta2  =  2; 

PDF 

kappa2  =  0 ; 

Scaling 

q, 

o 

lamda2  =  alpha2^2* (nxa2+kappa2) -nxa2; 
wm2  =  (1/ (2* (nxa2+lamda2 ) ) ) *ones (n2, 1) ; 
wc2  =  wm2 ; 

wm2 ( 1 )  =  lamda2 / (nxa2+lamda2 ) ; 

Weight 

wc2(l)  =  wm2 ( 1 )  +  (l-alpha2^2+beta2) ; 

Weight 

eta2  =  sqrt (nxa2+lamda2 ) ; 

q, 

o 


%  Number  of  INS  States 
%  Process  Noise  States 
%  Measurement  Noise 

%  Total  Augmented 

%  Total  Iterations 
%  Tunable,  0<alpha<l 
%  beta  =  2  for  Gaussian 

%  Tunable  Secondary 

Factor . 

%  Weighting  Factor 
%  Measurement  Weights 
%  Covariance  Weights 
%  Zeroth  Measurement 

%  Zeroth  Covariance 

%  Covariance  Weighting 
Factor 


end 

%%  AHRS  ESTIMATOR 

%  Build  Augmented  State  Vector 

X  =  zeros (nxa,  n) ; 

xa  =  [x  bar;  zeros (nxa-nx, 1 )] ; 

%  Build  Augmented  Covariance  Matrix 
Sxa  =  [Sx  zeros (nx,nQ)  zeros (nx, nR) ; 

zeros (nQ,nx)  Qv  zeros (nQ, nR) ; 
zeros (nR,nx)  zeros (nR,nQ)  R]  ; 

%  Build  n+2  Sigma  Vectors 
c  =  eta*Sxa; 
for  k  =  1 : nxa+1 
if  k  ==  1 

X ( : , k)  =  xa; 

else 

X(:,k)  =  xa  +  c(k-l,:)'; 
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X(:,k;+nxa)  =  xa  -  c(k;-l,:)'; 

end 

end 

%  Time  Update 
Xx  =  zeros (nx,  n) ; 

X  bar  =  zeros (nx,  1); 
for  k  =  1 : n 

%  Gyro  Model  w  bi (true)  =  w_bi (measured)  -  bias  -  noise 
w_bi_b  =  [p-X ( 5 ,  k) -X ( 8  ,  k)  ;  q-X (6,  k) -X (9,  k)  ;  r-X(7,  k)-X(10,k)] 

if  SIM  ==  1 

%  No  Sidereal  Rate 
w  bt  =  w  bi  b; 

else 

%  Sidereal  Rate  Included 
R_t2b  =  rot_t2b (X ( 1 : 4 , k) ) ; 

w_bt  =  w_bi_b  -  R_t2b* [wei*cos (latO) ;  0;  -wei*sin (latO) ] ; 

end 

%  Quaternion  Update 

wl  =  w  bt(l);  w2  =  w  bt(2);  w3  =  w  bt(3); 

Q  kpl  =  X(l:4,  k)  +  (dt/2)*[0,  -wl,  -w2,  -w3; 

wl,  0,  w3,  -w2; 

w2 ,  -w3,  0,  wl; 

w3,  w2 ,  -wl,  0]*X(1:4,  k) ; 

%  Quaternion  Normalization 
Q  kpl  n  =  Q  kpl/sqrt(Q  kpl'*Q  kpl); 

%  Gyro  Bias  Update 
if  SIM  ==  1 

%  Constant  Bias 
bg  =  X  ( 5  :  7 ,  k)  ; 

else 

%  Random  Walk  Bias 

bg  =  X(5:7,k)  +  dt*X(ll:13,k); 

end 

%  Rebuild  the  State  Vector  for  Each  Iteration 
Xx(:,k)  =  [Q_kpl_n;  bg] ; 

%  Calculated  the  Weighted  Mean  of  the  State  Vector 

X  bar  =  X  bar  +  wm ( k) *Xx ( : , k) ; 

end 

%  Calculate  Error  Covariance 

Ax  =  sqrt (wc (2 ) ) * (Xx ( : , 2 : n)  -  x_bar ( : , ones ( 1 , n-1 ) ) ) ; 

%  QR  Decomposition 
[~,  sx]  =  qr (Ax' , 0) ; 

%  Cholesky  Update  (Downdate  for  negative  zeroth  weight) 

Sx  =  cholupdate ( sx,  sqrt ( -wc ( 1 ) ) * (Xx ( : , 1 ) -x_bar ) ,  '-'); 
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%  Non-Linear  Measurement  Process  Equations 
Y  =  zeros (nR,  n) ; 
y_bar  =  zeros (nR, 1 ) ; 

Heading  =  zeros (l,n); 
if  isempty(Hl) 

HI  =  zeros ( 1 , n) ; 

H2  =  zeros ( 1 , n) ; 

HeadingO  =  zeros (l,n); 
ii  =  zeros ( 1 , n) ; 

end 

for  k  =  l:n 

%  Rotation  Matrix  (LTP  to  Body) 

R  t2b  =  rot  t2b (Xx ( 1 ; 4 , k) ) ; 

%  Gyro  Measurements 
w_bi  =  [p;  q;  r]  -  Xx(5:7,k); 
if  SIM  ==  1 

noise  R  =  X(ll:14,k); 
w  bt  =  w  bi; 

else 

noise  R  =  X(14;17,k); 
w_bt  =  w_bi  -  R_t2b* [wei*cos (latO) ;  0;  -wei*sin (latO) ] ; 

end 

%  Quaternion  to  Euler  Angles 

qO  =  Xx(l,k);  ql  =  Xx(2,k);  q2  =  Xx(3,k);  q3  =  Xx(4,k); 

psi  =  mod  (atan2  (2*  (ql*q2+q0*q3)  ,  q0''2+ql''2-q2''2-q3''2)  +  noise  R(4), 

2*pi) ; 

%  Unwrap  Heading  Angle  From  [0,  2*pi]  Range  to  Avoid  Jumps 
if  nn  ==  1 

HI (k)  =  psi; 

HeadingO (k)  =  Hl(k); 

Heading (k)  =  Hl(k); 

else 

H2 (k)  =  psi; 

if  (H2 (k) -HI (k) )  <=  -100*pi/180 
ii(k)  =  ii(k)+l; 

end 

if  (H2 (k) -HI (k) )  >=  100*pi/180 
ii (k)  =  ii (k) -1; 

end 

Heading (k)  =  HeadingO (k) +( (H2 (k) +2*pi*ii (k) )  -  HeadingO ( k) ) ; 

HI (k)  =  H2 (k) ; 

end 

%  Accelerometer  Estimate 

FB_hat  =  R_t2b* [ax_gps;  ay_gps;  az_gps]  +  ... 

cross (w_bt,  R_t2b*x_bar2 (4 : 6) )  -  ... 

R_t2b*[0;  0;  g] -x_bar2 ( 7 : 9 ) -noise_R ( 1 : 3 ) ; 

%  Calculated  Observations 
Y ( ; , k) = [FB_hat;  Heading (k)]; 


%  Pre-fill 
%  Pre-fill 
%  Pre-fill 

%  Pre-fill 
%  Pre-fill 
%  Pre-fill 
%  Pre-fill 
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%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm ( k) *Y ( : , k) ; 
end 

%  Estimated  Observation  Covariance 

Bx  =  sqrt (wc (2 ) ) * (Y ( :  ,  2 : n)  -  y_bar ( : , ones ( 1 , n-1 ) ) ) ; 

%  QR  Decomposition 

[~,  sy]  =  qr(Bx',0);  %  **"qr"  PRODUCES  UPPER  TRIANGULAR  MATRIX** 

%  Cholesky  Update  (or  Downdate) 

Sy  =  cholupdate ( sy,  sqrt ( -wc ( 1 ) ) * ( Y ( : , 1 ) -y_bar ) ,  ; 

%  ***Sy  MUST  BE  LOWER  TRIANGULAR*** 

Sy  =  Sy'  ; 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx, nR) ; 
for  k  =  l:n 

Pxy  =  Pxy  +  wc ( k) * ( (Xx ( : , k)  -  x  bar) * (Y ( : , k)  -  y  bar)'); 

end 

%  Measurement  Method  2  (Accelerometer) 

%  Heading  Unwrap  from  [0,  2*pi] 
if  nn  ==  1 

H3  =  heading; 

H4  =  0; 
psiO  =  H3; 
psi  m  =  H3; 

j  j=0; 

else 

H4  =  heading; 
if  (H4-H3)  <=  -100*pi/180 

jj  =  jj+1; 

end 

if  (H4-H3)  >=  100*pi/180 

jj  =  jj-1; 

end 

psi_m  =  psi0+ ( (H4+2*pi* j j )  -  psiO); 

H3  =  H4; 

end 

%  Measurement  Vector 
Z  =  [fx;  fy;  fz;  psi  m]  ; 

%  Kalman  Filter  Equations 
K  =  (Pxy/Sy' ) /Sy; 

Squares 

X  bar  =  X  bar  +  K* (Z  -  y  bar) ; 

%  Covariance  Corrections 
Ux  =  K*Sy; 
for  k  =  1 : nR 

Sx  =  cholupdate ( Sx,  Ux ( : ,  k)  ,  '-' )  ;  %  Cholesky  Downdate 

end 


%  Efficient  Least 
%  Kalman  Correction 
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Sx  =  Sx' ; 
TRIANGULAR 


MUST  BE  LOWER 


nn  =  2 ; 

%%  INS  ESTIMATOR 

%  Build  Augmented  State  Vector 

X  =  zeros (nxa2,  n2); 

xa  =  [x  bar2;  zeros (nxa2-nx2 , 1 )] ; 

%  Build  Augmented  Covariance  Matrix 

Sxa2  =  [Sx2  zeros (nx2 , nQ2 )  zeros (nx2 , nR2 ) ; 

zeros (nQ2 , nx2 )  Qv2  zeros (nQ2 , nR2 ) ; 
zeros (nR2 , nx2 )  zeros (nR2 , nQ2 )  R2 ]  ; 

%  Construct  n+2  SIGMA  Vectors 
c  =  eta2*Sxa2; 
for  k  =  l:nxa2+l 
if  k  ==  I 

X ( : , k)  =  xa; 

else 

X(:,k)  =  xa  +  c(k-l,:)'; 

X(:,k+nxa2)  =  xa  -  c(k-l,:)'; 

end 

end 

%  Time  Update 

Xx  =  zeros (nx2,  n2);  %  Pre-fill 

X  bar2  =  zeros (nx2,  1);  %  Pre-fill 

for  k  =  1 : n2 
%  Rotation  Matrix 
R  t2b  =  rot  t2b(x  bar (1:4)); 

R  b2t  =  R  t2b' ; 

%  Position  Update 

P_kpl  =  X(l:3,k)  +  dt*  [X  ( 4  :  5 ,  k)  ;  -X(6,k)]; 

%  Velocity  and  Bias  Update 
if  SIM  ==  1 
%  No  Sidereal 

V  kpl  =  X (4 : 6, k)  +  dt* (R  b2t* ( [fx;  fy;  fz]  -  X (7 : 9, k)  -  ... 

X(10:12,  k) )  +  [0;  0;  g] ) ; 
ba_kpl  =  X(7:9,k); 

else 

%  Sidereal  Included 

V  kpl  =  X(4:6,k)  +  dt* (R  b2t* ( [fx;  fy;  fz]  -  X(7:9,k)  -  ... 

X(10:12,  k) )  +  [0;  0;  g]  -  2*cross ( [wei*cos (latO) ;  ... 

0;  -wei*sin (latO) ] ,  X(4:6,k))); 
ba_kpl  =  X(7:9,k)  +  dt*X(13:15,k); 

end 

%  States 

Xx ( : , k)  =  [P  kpl;  V  kpl;  ba  kpl]; 
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%  Calculated  Mean 

X  bar2  =  x  bar2  +  wm2 ( k) *Xx ( :  ,  k)  ; 
end 

%  Predicted  Error  Covariance 

Ax  =  sqrt (wc2 (2 ) ) * (Xx ( :  ,  2 : n2 )  -  x  bar2 ( : , ones ( 1 , n2-l ) ) ) ; 

%  QR  Decomposition 
[~,  Sx2]  =  qr  (Ax'  ,  0)  ; 

%  Cholesky  Factor  Update 

Sx2  =  cholupdate ( Sx2 ,  sqrt ( -wc2 ( 1 ) ) * (Xx ( : , 1 ) -x_bar2 ) , 

%  Measurement  Equations 
Y  =  zeros (nR2,  n2); 
y_bar  =  zeros (nR2 , 1 ) ; 
for  k  =  1 : n2 

if  SIM  ==  1 

%  Calculated  Observations 
Y(;,k)=Xx(l:6,k)+X(13:18,k)  ; 

else 

%  Calculated  Observations 
Y(;,k)=Xx(l:6,k)+X(16:21,k)  ; 

end 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm2 ( k) *Y ( : , k) ; 
end 

%  Estimated  Observation  Covariance 
Bx  =  sqrt (wc2 (2 ) ) * (Y ( : , 2 : n2 )  -  y_bar ( : , ones ( 1 , n2-l ) ) ) ; 

%  QR  Decomposition 
[~,  Sy]  =  qr (Bx' , 0) ; 

%  Cholesky  Factor  Update 

Sy  =  cholupdate ( Sy,  sqrt ( -wc2 ( 1 ) ) * ( Y ( : , 1 ) -y_bar ) , 

%  Make  Sy  Lower  Triangular 
Sy  =  Sy' ; 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx2 , nR2 ) ; 
for  k  =  1 : n2 

Pxy  =  Pxy  +  wc2 ( k) * ( (Xx ( : , k)  -  x  bar2 ) * ( Y ( : , k)  -  y  bar)'); 

end 

%  Measurements 

Z  =  [N_sat;  E_sat;  D_sat;  speed*cos (CoG) ;  speed*sin (CoG)  ;  Vd_sat] 

%  Kalman  Filter  Equations 
%  Kalman  Gain  w/  Efficient  Least  Squares 
K  =  (Pxy/Sy' ) /Sy; 

%  Kalman  State  Corrections 
X  bar2  =  x  bar2  +  K* (Z  -  y  bar)  ; 

%  Covariance  Correction 
Ux  =  K*Sy; 
for  k  =  1 : nR2 


%  Pre-fill 
%  Pre-fill 
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Sx2  =  cholupdate ( Sx2 ,  Ux ( : , k) , 

end 

%  Make  Sx2  Lower  Right  Triangular 
Sx2  =  Sx2 ' ; 

X  HAT  =  [x  bar (1:4);  x  bar2(l:6);  x  bar  (5: 7);  x  bar2(7:9)]; 
end 

kk=kk+l; 

end 

function  R  t2b  =  rot  t2b(x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

R_t2b  =  [q0^2  +  ql''2-q2^2-q3''2  2*  (ql*q2  +  q0*q3)  2*(ql*q3-q0*q2); 

2* (q2*ql-q0*q3)  qO ^2 -ql ^2+q2 ^2 -q3 ^2  2*(q2*q3+q0*ql); 

2* (q3*ql  +  q0*q2)  2  * ( q3 *q2 -qO *ql )  qO ^2 -ql ^2 -q2 ^2  +  q3 ^2 ] ; 

end 

function  q  =  e2q(phi,  theta,  psi) 

%  Form  Rotation  Matrix 

R_psi  =  [cos (psi)  sin (psi)  0;  -sin (psi)  cos (psi)  0;  0  0  1]; 

R_theta  =  [cos (theta)  0  -sin (theta)  ;  0  1  0;  sin (theta)  0  cos  (theta) ] ; 
R_phi  =[10  0;  0  cos (phi)  sin (phi);  0  -sin (phi)  cos  (phi)]; 

R  n2b  =  R  phi*R  theta*R  psi; 

%  Extract  Quaternions 
q0=sqrt (1+trace (R_n2b) ) /2; 
ql= (R_n2b (2,3) -R_n2b (3,2) ) / (4*q0) ; 
q2= (R_n2b (3, 1) -R_n2b (1, 3) ) / (4*q0)  ; 
q3= (R_n2b (1, 2) -R_n2b (2,1))/ (4*q0)  ; 

q  =  [qO;  ql;  q2 ;  q3]; 
end 

E.  SPHERICAL  SIMPLEX  UNSCENTED  KALMAN  FILTER  MATLAB 
CODE 

1,  Measurement  Model  One  Implementation 


function  X_HAT  =  SSUKFl (u) 

%%  SPHERICAL  SIMPLEX  UNSCENTED  KALMAN  FILTER 
%  Measurement  Model  1 
%  LT  Steven  Terjesen 
%  September  2014 

%  This  estimator  is  built  for  use  in  MATLAB  Simulink.  There  are  38 
inputs 

%  required  and  can  be  run  in  Simulink  with  an  "Interpolated  MATLAB 
%  Function"  block.  The  first  13  inputs  are  the  vehicle  data:  Course 
Over 

%  Ground,  Speed  Over  Ground,  Accelerometer  Measurements,  Gyro 
Measurements , 
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%  GPS  Measurements  (In  LTP  XNorth-YEast-ZDown) ,  Heading  Measurement, 
and 

%  Vertical  Velocity  (zeroed  out  for  SEAFOXII  data) .  Inputs  14  through 
34 

%  are  the  Process  Noise  and  Measurement  Noise  diagonal  elements.  These 
%  elements  are  not  hard  coded  to  allow  for  easier  tuning.  Inputs  35 
%  through  37  are  the  N-E-D  accelerations  in  the  LTP  frame.  Input  38  is 
a 

%  toggle  for  selecting  whether  the  data  is  from  Condor  or  SEAFOX  II. 

%%  System  Inputs 
%  Measurement  inputs 


CoG  =  u ( 1 ) ; 

%GPS 

Course  Over  Ground 

speed  =  u(2); 

%GPS 

Speed  Over  Ground 

fx  =  u  ( 3 )  ;  f  y 

=  u(4) 

;  f  z  =  u  ( 5 )  ; 

%IMU 

Accelerations 

p  =  u  (  6 )  ;  q  = 

u(7)  ; 

r  =  u  ( 8 )  ; 

%IMU 

Angular  Rates 

N  sat  =  u ( 9 ) ; 

E  sat 

=  u(10);  D  sat  =  u(ll); 

%GPS 

Position  (NED) 

heading  =  u ( 12 ) ; 

%GPS 

Heading 

Vd  sat  =  u(13) 

'  ; 

%GPS 

LTP  Down  Velocity 

%  Process  and  Measurement  Noise  Matrices 
R  =  diag (u (14 : 16) ) ; 

R2  =  diag (u  ( 17  :  22 ) )  ; 

SIM  =  u (38) ; 

SEAFOXII (2) 


%AHRS  Measurement  Noise 
%INS  Measurement  Noise 

%Condor(l)  or 

Simulation  Selector 


if  SIM  ==  1 

Qv  =  diag (u (23 : 25) ) ; 
Condor 

Qv2  =  diag (u (29 : 31) ) ; 
Condor 
else 

Qv  =  diag (u (23 : 2 8 ) ) ; 
SEAFOX 

Qv2  =  diag (u (2  9 : 34) )  ; 
SEAFOX 

Vd_sat  =  0; 

q, 

o 

on 

q, 

o 

D_sat  =  0; 

Measurement 

q, 

o 

set 

q, 

o 

end 


%AHRS  Process  Noise 
%INS  Process  Noise 

%AHRS  Process  Noise 

%INS  Process  Noise 

%No  Vertical  Velocity 
measurement  available 

SEAFOX  II,  set  Vd=0. 
%No  Altitude 

available  on  SEAFOXII, 

D  sat  =  0; 


%  GPS  Accelerations 
ax  gps  =  u(35);  ay  gps 


u(36);  az_gps  =  u(37);  %GPS  Accelerations  as 

%  calculated  from  3rd 
%  Order  Filter 
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%%  Initialization 

persistent  x  bar  HeadingO  HI  H2  ii  j j  psiO  H3  H4  ... 

Px  Px2  kk  dt  g  nn  nx  nxa  nQ  nR  n  wc  wm  x  bar2  nx2  nxa2  n2  nQ2  nR2  wc2 
wm2  sig  sig2  wei  latO 

%  Allows  time  for  Condor  To  steady  out  during  live  testing 
if  isempty(kk) 
kk  =  0; 

end 

if  kk  <  1  &&  SIM==1 

X_HAT  =  zeros (16,1); 

else 

if  isempty(x  bar) 

%  Miscellaneous 
dt  =  0.01; 
g  =  9.8; 

[m/s''2  ] 

wei  =  7.292115*10^-5; 
lat0=  0.639268394832413; 

%lon0  =  -2.115435878466264 
nn=l ; 

Initializer 

%  AHRS  Initialization 
%  Euler  Angle  Initialization 

rx  =  ax  gps*cos (heading) +ay  gps*sin (heading) ; 
ry  =  -ax_gps*sin (heading) +ay_gps*cos (heading)  ; 
rz  =  az_gps-g; 

theta  0  =  atan((-rx*rz  -  fx*sqrt (rx^2+rz^2-fx^2) ) / (fx^2-rz^2) ) ; 
r_theta  =  rx*sin (theta_0)  +  rz*cos (theta_0) ; 
fc  =  f y- speed* r; 

phi  0  =  atan((r  theta*ry  +  fc*sqrt (ry^2+r  theta^2-fc^2) ) / (fc^2- 
r_theta''2 )  )  ; 
psi  0=heading; 

%  Euler  Angles  to  Quaternions 
q_0  =  e2q(phi_0,  theta_0,  psi_0); 

%  Gyro  Bias  Initial  Values 
bg_0  =  [ 0 ;  0 ;  0 ] ; 

%  Inital  State  Vector 
x_bar  =  [q_0;  bg_0] ; 

%  Initial  Covariance  Estimate 
Px  =  diag ( [ le-5*ones ( 1 , 4 )  le-3*ones (1, 3) ] ) ; 

%INS  Initialization 

%  Position  &  Velocity  Initialization 
x_0  =  N_sat; 
y_0  =  E_sat; 
z_0  =  D_sat; 
vn_0  =  speed*cos (CoG) ; 
ve_0  =  speed*sin (CoG) ; 
vd_0  =  Vd_sat; 

%  Accelerometer  Initial  Bias  Estimate 
ba  0  =  [ 0 ;  0 ;  0 ] ; 


%Filter  dt  [sec] 
%Gravity  Constant 

%Sidereal  Rate  [rad/s] 
%Origin  of  LTP  [rad] 

%Heading  Count 
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%  Initial  State  Vector 

x_bar2  =  [x_0;  y_0;  z_0;  vn_0;  ve_0;  vd_0;  ba_0] ; 

%  Initial  Covariance  Estimate 

Px2  =  diag ( [le-5*ones (1, 3) ,  le-5*ones (1, 3) ,  le-3*ones (1, 3) ] ) ; 


%  AHRS  SIGMA  Weights  Initialization 
nx  =  length (x  bar); 
nQ  =  length (diag (Qv) ) ; 
nR  =  length (diag (R) ) ; 

States 

nxa  =  nx+nQ+nR; 

States 
n  =  nxa+2; 
alpha  =  le-3; 

Factor 
beta  =  2; 

PDF 

WxO  =  1/3; 

Wx  =  zeros (n, 1 ) ; 

%  Simplex  Weights 


k  =  l:n 

if  k  ==  1 

Wx  ( k) 

=  WxO; 

else 

Wx  ( k) 

=  (1-WxO) / (nxa+1) ; 

end 

end 

%  Scaled  Simplex  Weights 
for  k  =  l:n 
if  k  ==  1 


%  Number  of  AHRS  States 
%  Process  Noise  States 
%  Measurement  Noise 

%  Total  Augmented 

%  Number  of  Iterations 
%  Tunable  Scaling 

%  beta  =  2  for  Gaussian 

%  Tunable,  0<W<1 
%  Pre-fill 


wx(k)=  1+ (Wx ( k) -1 ) /alpha^2  ; 

else 

wx(k)  =  Wx ( k) /alpha^2 ; 

end 


end 

%  Incorporate  prior  PDF  knowledge  for  Higher  Order  Moments 
for  k  =  l:n 
if  k  ==  1 


wm  ( k) 
wc  ( k) 

else 

wm  ( k) 
wc  ( k) 

end 


end 


wx ( k ) ; 

wx (k) + ( l-alpha^2+beta) ; 

wx ( k ) ; 
wx ( k ) ; 


%Sigma  Matrix 

sig  =  zeros (nxa,  n) ; 

sig(l,l)  =  0; 

sig(l,2)  =  -1/sqrt (2*wx (2) ) ; 
sig(l,3)  =  1/sqrt (2*wx (2) ) ; 
for  j  =  2 : nxa 

for  i  =  1:1 

sig(j,i)  =  0; 

end 


%  Pre-fill 
%  Initialize  (1,1) 
%  Initialize  (1,2) 
%  Initialize  (1,3) 
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for  i  =  2 : j  +1 

sig(j,i)  =  -1/sqrt ( j * ( j+1) *wx (2) ) ; 

end 

for  i  =  j  +2 : j  +2 

sig(j,i)  =  j /sqrt ( j * ( j+1) *wx (2) ) ; 

end 

end 


%  Number  of  INS  States 
%  Process  Noise  States 
%  Measurement  Noise 

%  Total  Augmented 

%  Total  Iterations 
%  Tunable,  0<alpha<l 
%  beta  =  2  for  Gaussian 

%  Tunable,  0<W<1 
%  Pre-fill 

%  Sigma  Weights 


k  =  1 : n2 

if  k  ==  1 

Wx2 ( k) 

=  Wx02; 

else 

Wx2 ( k) 

=  (1-Wx02) / (nxa2+l) ; 

end 

end 

%  Scaled  Sigma  Weights 
for  k  =  1 : n2 
if  k  ==  1 

wx2(k)=  1+ (Wx2  ( k) -1 ) /alpha''2  ; 

else 

wx2(k)  =  Wx2  ( k) /alpha''2  ; 

end 

end 


%  INS  SIGMA  Weights  Initialization 
nx2  =  length (x  bar2); 
nQ2  =  length (diag (Qv2 )) ; 
nR2  =  length (diag (R2 )) ; 

States 

nxa2  =  nx2+nQ2+nR2 ; 

States 

n2  =  nxa2+2; 
alpha  =  le-3; 
beta  =  2; 

PDF 

Wx02  =  1/3; 

Wx2  =  zeros (n2 , 1 ) ; 


Incorporate  prior  PDF  knowledge  for  Higher  Order  Moments 


else 


1  :n2 

wm2 ( k ) 

=  wx2 ( k) ; 

wc2 ( k) 

=  wx2  (k)  +  (l-alpha''2+beta)  ; 

wm2 ( k ) 

=  wx2 ( k) ; 

wc2 ( k) 

=  wx2 ( k) ; 

end 


end 


%  Sigma  Matrix 

sig2  =  zeros (nxa2,  n2); 

sig2 (1,1)  =  0 ; 


%  Pre-fill 
%  Initialize  (1,1) 
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sig2(l,2)  =  -1/sqrt (2*wx2 (2) ) ;  %  Initialize  (1,2) 

sig2(l,3)  =  1/sqrt (2*wx2 (2) ) ;  %  Initialize  (1,3) 

for  j  =  2 : nxa2 
for  i  =  1 : 1 

sig2 ( j , i)  =  0; 

end 

for  i  =  2 : j  +1 

sig2(j,i)  =  -1/sqrt (j * (j+1) *wx2 (2) ) ; 

end 

for  i  =  j  +2 : j  +2 

sig2(j,i)  =  j /sqrt ( j * ( j+1) *wx2 (2) ) ; 

end 

end 

end 

%%  AHRS  ESTIMATOR 

%  Build  Augmented  State  Vector 

X  =  zeros (nxa,  n) ; 

xa  =  [x  bar;  zeros (nxa-nx, 1 )] ; 

%  Build  Augmented  Covariance  Matrix 
Pxa  =  [Px  zeros (nx,nQ)  zeros (nx, nR) ; 

zeros (nQ,nx)  Qv  zeros (nQ, nR) ; 
zeros (nR,nx)  zeros (nR,nQ)  R]  ; 

%  Build  n+2  Sigma  Vectors 
for  k  =  1 : n 

X ( : , k)  =  xa  +  Pxa*sig ( : , k) ; 

end 

%  Time  Update 
Xx  =  zeros (nx,  n) ; 

X  bar  =  zeros (nx,  1); 
for  k  =  1 : n 

%  Gyro  Model  w  bi (true)  =  w_bi (measured)  -  bias  -  noise 
w_bi_b  =  [p-X ( 5 ,  k) -X ( 8  ,  k)  ;  q-X (6,  k) -X (9,  k)  ;  r-X(7,  k) -X ( 1 0 , k) ] ; 

if  SIM  ==  1 

%  No  Sidereal  Rate 
w  bt  =  w  bi  b; 

else 

%  Sidereal  Rate  Included 
R_t2b  =  rot_t2b (X ( 1 : 4 , k) ) ; 

w_bt  =  w_bi_b  -  R_t2b* [wei*cos (latO) ;  0;  -wei*sin (latO) ] ; 

end 

%  Quaternion  Update 


wl  =  w  bt(l);  w2  =  w  bt(2); 

w3  = 

w  bt  ( 3 

)  ; 

Q_kpl  =  X(l:4,  k)  +  Tdt/2)* 

[0, 

-wl , 

w2 ,  -w3; 

( — 1 

0, 

w3. 

-w2 ; 

w2 , 

-w3. 

0, 

wl ; 

w3. 

w2 , 

\ — 1 

1 

0]*X(1:4,  k); 
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%  Quaternion  Normalization 
Q  kpl  n  =  Q  k;pl/sqrt(Q  k;pl'*Q  kpl); 

%  Gyro  Bias  Update 
if  SIM  ==  1 

%  Constant  Bias 
bg  =  X  ( 5  :  7 ,  k)  ; 

else 

%  Random  Walk  Bias 
bg  =  X(5:7,k)  +  dt*X(ll:13,k); 

end 

%  Rebuild  the  State  Vector  for  Each  Iteration 
Xx(:,k)  =  [Q_kpl_n;  bg] ; 

%  Calculated  the  Weighted  Mean  of  the  State  Vector 
X  bar  =  X  bar  +  wm ( k) *Xx ( : , k) ; 
end 

%  Calculate  Error  Covariance 
Px=  zeros (nx,  nx) ; 
for  k  =  1 : n 

Px  =  Px  +  wc ( k) * ( (Xx ( : ,  k)  -  x  bar ) * (Xx ( : ,  k)  -  x  bar)'); 

end 

%  Non-Linear  Measurement  Process  Equations 
Y  =  zeros (nR,  n) ; 
y_bar  =  zeros (nR, 1 ) ; 

Heading  =  zeros (l,n); 
if  isempty(Hl) 

HI  =  zeros ( 1 , n) ; 

H2  =  zeros ( 1 , n) ; 

Headings  =  zeros (l,n); 
ii  =  zeros ( 1 , n) ; 

end 

for  k  =  1 : n 

if  SIM  ==  1 

noise  R  =  X(ll:13,k); 

else 

noise  R  =  X(14:16,k); 

end 

%  Quaternion  to  Euler  Angles 
qO  =  Xx(l,k);  ql  =  Xx(2,k);  q2  =  Xx(3,k);  q3  =  Xx(4,k); 
phi  =  atan2  (2*  (q2*q3+q0*ql)  ,  q0''2-ql''2-q2''2+q3''2)  +  noise  R  ( 1 )  ; 
theta  =  asin (-2* (ql*q3-q0*q2) ) +  noise_R(2); 

psi  =  mod  (atan2  (2*  (ql*q2+q0*q3)  ,  q0''2+ql''2-q2''2-q3''2)  +  noise  R(3), 
2*pi) ; 

%  Unwrap  Heading  Angle  From  [0,  2*pi]  Range  to  Avoid  Jumps 
if  nn  ==  1 

HI (k)  =  psi; 


%  Pre-fill 
%  Pre-fill 
%  Pre-fill 

%  Pre-fill 
%  Pre-fill 
%  Pre-fill 
%  Pre-fill 
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HeadingO ( k)  =  Hl(k); 

Heading (k)  =  Hl(k); 

else 

H2 (k)  =  psi; 

if  (H2 (k) -HI (k) )  <=  -100*pi/180 
ii(k)  =  ii(k)+l; 

end 

if  (H2 (k) -HI (k) )  >=  100*pi/180 
ii (k)  =  ii (k) -1; 

end 

Heading (k)  =  HeadingO (k) +( (H2 (k) +2*pi*ii (k) )  -  HeadingO (k)) 

HI (k)  =  H2 (k) ; 

end 

%  Calculated  Observations 
Y ( : , k) = [phi ;  theta;  Heading ( k) ] ; 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm ( k) *Y ( : , k) ; 
end 

%  Estimated  Observation  Covariance 
Py  =  zeros (nR, nR) ; 
for  k  =  1 : n 

Py  =  Py  +  wc ( k) * ( ( Y ( : , k)  -  y_bar) * (Y ( : , k)  -  y_bar) ' ) ; 

end 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx,  nR)  ; 
for  k  =  l:n 

Pxy  =  Pxy  +  wc ( k) * ( (Xx ( : , k)  -  x  bar) * (Y ( : , k)  -  y  bar)'); 

end 

%  Measurement  Method  1  (Accelerometer-GPS-Compass  Fusion) 

%  Rotate  GPS  XYZ  Acceleration  about  Heading 
rx  =  ax  gps*cos (heading) +ay  gps*sin (heading) ; 
ry  =  -ax_gps*sin (heading) +ay_gps*cos (heading) ; 
rz  =  az_gps-g; 

%  Computer  Pitch 

theta  =  atan((-rx*rz  -  fx*sqrt (rx^2  +  rz^2-fx^2 ) )  /  (fx^2-rz^2) )  ; 

%  Compute  Roll 

r_theta  =  rx*sin (theta)  +  rz*cos (theta) ; 

fc  =  fy-(norm(x  bar2 (4 : 6) ) ) *r;  %  Added  Coriolis  Term 

phi  =  atan((r  theta*ry  +  fc*sqrt  (ry''2+r  theta''2-fc''2 )  )  /  (fc''2- 

r_theta^2 ) ) ; 

%  Heading  Unwrap  from  [0,  2*pi] 
if  nn  ==  1 

H3  =  heading; 

H4  =  0; 
psiO  =  H3; 
psi  m  =  H3; 

j  j=0; 

else 
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H4  =  heading; 
if  (H4-H3)  <=  -100*pi/180 

jj  =  jj+1; 

end 

if  (H4-H3)  >=  100*pi/180 

jj  =  jj-1; 

end 

psi_m  =  psiO+ ( (H4+2*pi* j j )  -  psiO); 

H3  =  H4; 

end 

%  Measurement  Vector 
Z  =  [phi;  theta;  psi_m] ; 

%  Kalman  Filter  Equations 
K  =  Pxy/Py; 

X  bar  =  X  bar  +  K* (Z  -  y  bar) ; 

%  Covariance  Corrections 
Px  =  Px  -  K*Py*K'; 

nn  =  2 ; 

%%  INS  ESTIMATOR 

%  Build  Augmented  State  Vector 
X  =  zeros (nxa2,  n2); 
xa  =  [x  bar2;  zeros (nxa2-nx2 , 1 )] ; 

%  Build  Augmented  Covariance  Matrix 
Pxa2  =  [Px2  zeros (nx2 , nQ2 )  zeros (nx2 , nR2 ) ; 

zeros (nQ2 , nx2 )  Qv2  zeros (nQ2 , nR2 ) ; 
zeros (nR2 , nx2 )  zeros (nR2 , nQ2 )  R2 ]  ; 

%  Construct  n+2  SIGMA  Vectors 
for  k  =  1 : n2 

X ( : ,  k)  =  xa  +  Pxa2*sig2 ( : ,  k) ; 

end 

%  Time  Update 
Xx  =  zeros (nx2,  n2); 

X  bar2  =  zeros (nx2,  1); 
for  k  =  1 : n2 
%  Rotation  Matrix 
R  t2b  =  rot  t2b(x  bar(l:4)); 

R  b2t  =  R  t2b' ; 

%  Position  Update 
P_kpl  =  X(l:3,k)  +  dt*[X(4:5,k)  ;  -X(6,k)]; 

%  Velocity  and  Bias  Update 
if  SIM  ==  1 
%  No  Sidereal 

V_kpl  =  X(4:6,k)  +  dt* (R_b2t* ( [fx;  fy;  fz]  -  X(7:9,k)  - 


%  Pre-fill 
%  Pre-fill 


%  Kalman  Gain 
%  Kalman  Correction 
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X(10:12,  k))  +  [0;  0;  g] ) ; 
ba_kpl  =  X(7:9,k); 

else 

%  Sidereal  Included 

V_kpl  =  X(4:6,k)  +  dt* (R_b2t* ( [fx;  fy;  fz]  -  X(7:9,k)  -  ... 

X(10:12,  k) )  +  [0;  0;  g]  -  2 *cross ( [wei*cos ( latO ) ;  .. 

0;  -wei*sin (latO) ]  ,  X(4:6,k))); 
ba_kpl  =  X(7:9,k)  +  dt*X(13:15,k); 

end 

%  States 

Xx ( : , k)  =  [P  kpl;  V  kpl;  ba  kpl]; 

%  Calculated  Mean 

X  bar2  =  x  bar2  +  wm2 ( k) *Xx ( : , k) ; 
end 

%  Predicted  Error  Covariance 
Px2=  zeros (nx2,  nx2); 
for  k  =  1 : n2 

Px2  =  Px2  +  wc2 ( k) * ( (Xx ( : , k)  -  x  bar2 ) * (Xx ( : , k)  -  x  bar2)'); 

end 

%  Measurement  Equations 
Y  =  zeros (nR2,  n2); 
y_bar  =  zeros (nR2 , 1 ) ; 
for  k  =  1 : n2 

if  SIM  ==  1 

%  Calculated  Observations 
Y(:,k)=Xx(l:6,k)+X(13:18,k)  ; 

else 

%  Calculated  Observations 
Y  ( : ,  k)  =Xx  (1 :  6,  k)  +X  ( 1 6  :  2 1 ,  k)  ; 

end 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm2 ( k) *Y ( : , k) ; 
end 

%  Estimated  Observation  Covariance 
Py  =  zeros (nR2 , nR2 ) ; 
for  k  =  1 : n2 

Py  =  Py  +  wc2 ( k) * ( ( Y ( : , k)  -  y_bar) * (Y ( : , k)  -  y_bar) ' ) ; 

end 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx2 , nR2 ) ; 
for  k  =  1 : n2 

Pxy  =  Pxy  +  wc2 ( k) * ( (Xx ( : , k)  -  x  bar2 ) * ( Y ( : , k)  -  y  bar)'); 

end 

%  Measurements 

Z  =  [N_sat;  E_sat;  D_sat;  speed*cos (CoG) ;  speed*sin (CoG)  ;  Vd_sat] 
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%  Pre-fill 
%  Pre-fill 


%  Kalman  Filter  Equations 

K  =  Pxy/Py;  %  Kalman  Gain 

%  Kalman  State  Corrections 
X  bar2  =  x  bar2  +  K* (Z  -  y  bar) ; 

%  Covariance  Correction 
Px2  =  Px2  -  K*Py*K'; 

X  HAT  =  [x  bar (1:4);  x  bar2(l:6);  x  bar (5: 7);  x  bar2(7:9)]; 
end 

kk=kk+l ; 

end 

function  R  t2b  =  rot  t2b(x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

R_t2b  =  [q0^2+ql^2-q2^2-q3^2  2* (ql*q2+q0*q3)  2*(ql*q3-q0*q2); 

2*  (q2*ql-q0*q3)  qO ^2 -ql -'2+q2 ^2 -q3 ''2  2*(q2*q3+q0*ql); 

2*  (q3*ql  +  q0*q2)  2  *  ( q3  *q2 -qO  *ql )  qO  ^2 -ql -^2 -q2  ^2  +  q3 ''2  ]  ; 

end 

function  q  =  e2q(phi,  theta,  psi) 

%  Form  Rotation  Matrix 

R_psi  =  [cos (psi)  sin (psi)  0;  -sin (psi)  cos (psi)  0;  0  0  1]; 

R_theta  =  [cos (theta)  0  -sin  (theta) ;  0  1  0;  sin  (theta)  0  cos (theta) ] 
R_phi  =[10  0;  0  cos (phi)  sin (phi);  0  -sin (phi)  cos  (phi)]; 

R  n2b  =  R  phi*R  theta*R  psi; 

%  Extract  Quaternions 
q0=sqrt (1+trace (R_n2b) ) /2; 
ql= (R_n2b (2,3) -R_n2b (3, 2) ) / (4*q0) ; 
q2= (R_n2b (3,1) -R_n2b (1,3) ) / ( 4  *q0 ) ; 
q3= (R_n2b (1,2) -R_n2b (2, 1) ) / (4*q0)  ; 

q  =  [qO;  ql;  q2 ;  q3]; 
end 


2,  Measurement  Model  Two  Implementation 

function  X_HAT  =  SSUKF2 (u) 

%%  SPHERICAL  SIMPLEX  UNSCENTED  KALMAN  FILTER 
%  Measurement  Model  2 
%  LT  Steven  Terjesen 
%  September  2014 

%  This  estimator  is  built  for  use  in  MATLAB  Simulink.  There  are  39 
inputs 

%  required  and  can  be  run  in  Simulink  with  an  "Interpolated  MATLAB 
%  Function"  block.  The  first  13  inputs  are  the  vehicle  data:  Course 
Over 

%  Ground,  Speed  Over  Ground,  Accelerometer  Measurements,  Gyro 
Measurements , 

%  GPS  Measurements  (In  LTP  XNorth-YEast-ZDown)  ,  Heading  Measurement, 
and 
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%  Vertical  Velocity  (zeroed  out  for  SEAFOXII  data) .  Inputs  14  through 
35 

%  are  the  Process  Noise  and  Measurement  Noise  diagonal  elements.  These 
%  elements  are  not  hard  coded  to  allow  for  easier  tuning.  Inputs  36 
%  through  38  are  the  N-E-D  accelerations  in  the  LTP  frame.  Input  39  is 
a 

%  toggle  for  selecting  whether  the  data  is  from  Condor  or  SEAFOX  II. 

%%  System  Inputs 
%  Measurement  inputs 


CoG  =  u ( 1 ) ; 

%GPS 

Course  Over  Ground 

speed  =  u(2); 

%GPS 

Speed  Over  Ground 

fx  =  u  ( 3 )  ;  f  y 

=  u(4) 

;  f  z  =  u  ( 5 )  ; 

%IMU 

Accelerations 

p  =  u  (  6 )  ;  q  = 

u(7)  ; 

r  =  u  ( 8 )  ; 

%IMU 

Angular  Rates 

N  sat  =  u ( 9) ; 

E  sat 

=  u(10);  D  sat  =  u(ll); 

%GPS 

Position  (NED) 

heading  =  u ( 12 ) ; 

%GPS 

Heading 

Vd  sat  =  u(13) 

'  ; 

%GPS 

LTP  Down  Velocity 

%  Process  and  Measurement  Noise  Mat 
R  =  diag (u  ( 14  : 17 ) )  ; 

R2  =  diag (u ( 1 8 : 23 ) )  ; 

SIM  =  u  (39)  ; 

SEAFOXII  (2) 

g, 

o 

if  SIM  ==  1 

Qv  =  diag (u (24 : 26) )  ; 

Condor 

Qv2  =  diag (u ( 30 : 32 ) ) ; 

Condor 

else 

Qv  =  diag (u (24 : 29) ) ; 

SEAFOX 

Qv2  =  diag (u ( 30 : 35 ) ) ; 

SEAFOX 

Vd_sat  =  0; 

g, 

o 

on 

g, 

o 

D_sat  =  0; 

Measurement 

g, 

o 

set 

g, 

o 

end 

%  GPS  Accelerations 

ax_gps  =  u(36);  ay_gps  =  u(37);  az_ 


ices 

%AHRS  Measurement  Noise 
%INS  Measurement  Noise 

%Condor(l)  or 
Simulation  Selector 

%AHRS  Process  Noise 
%INS  Process  Noise 

%AHRS  Process  Noise 

%INS  Process  Noise 

%No  Vertical  Velocity 
measurement  available 

SEAFOX  II,  set  Vd=0 . 
%No  Altitude 

available  on  SEAFOXII, 

D_sat  =  0; 

ps  =  u(38);  %GPS  Accelerations  as 
%  calculated  from  3rd 
%  Order  Filter 


%%  Initialization 

persistent  x  bar  HeadingO  HI  H2  ii  jj  psiO  H3  H4  ... 
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Px  Px2  kk  dt  g  nn  nx  nxa  nQ  nR  n  wc  wm  x  bar2  nx2  nxa2  n2  nQ2  nR2  wc2 


wm2  sig  sig2  wei  latO 

%  Allows  time  for  Condor  To  steady  out  during  live  testing 
if  isempty(kk) 
kk  =  0; 

end 

if  kk  <  1  &&  SIM==1 

X  HAT  =  zeros (16,  1)  ; 

else 

if  isempty(x  bar) 

%  Miscellaneous 
dt  =  0.01; 
g  =  9.8; 

[m/s^2 ] 

wei  =  7.292115*10^-5; 
lat0=  0.639268394832413; 

%lon0  =  -2.115435878466264 
nn=l ; 

Initializer 

%  AHRS  Initialization 
%  Euler  Angle  Initialization 
rx  =  ax  gps*cos (heading) +ay  gps*sin (heading) ; 
ry  =  -ax_gps*sin (heading) +ay_gps*cos (heading) ; 
rz  =  az_gps-g; 

theta  0  =  atan((-rx*rz  -  fx*sqrt (rx^2  +  rz^2-fx^2) ) / (fx^2-rz^2) )  ; 
r_theta  =  rx*sin (theta_0)  +  rz*cos (theta_0) ; 
fc  =  f y- speed* r; 

phi  0  =  atan((r  theta*ry  +  fc*sqrt (ry^2+r  theta^2-fc^2) ) / (fc^2- 
r_theta''2 )  )  ; 
psi  0=heading; 

%  Euler  Angles  to  Quaternions 
q_0  =  e2q(phi_0,  theta_0,  psi_0); 

%  Gyro  Bias  Initial  Values 
bg_0  =  [0;  0;  0]  ; 

%  Inital  State  Vector 
x_bar  =  [q_0;  bg_0] ; 

%  Initial  Covariance  Estimate 

Px  =  diag ( [le-5*ones (1, 4)  le-3*ones (1, 3) ] ) ; 

%INS  Initialization 

%  Position  &  Velocity  Initialization 

x_0  =  N_sat; 

y_0  =  E_sat; 

z_0  =  D_sat; 

vn_0  =  speed*cos (CoG) ; 

ve_0  =  speed*sin (CoG) ; 

vd_0  =  Vd_sat; 

%  Accelerometer  Initial  Bias  Estimate 
ba_0  =  [ 0 ;  0 ;  0 ]  ; 

%  Initial  State  Vector 

x_bar2  =  [x_0;  y_0;  z_0;  vn_0;  ve_0;  vd_0;  ba_0] ; 
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%Filter  dt  [sec] 
%Gravity  Constant 

%Sidereal  Rate  [rad/s] 
%Origin  of  LTP  [rad] 

%Heading  Count 


%  Initial  Covariance  Estimate 

Px2  =  diag ( [le-5*ones (1, 3) ,  le-5*ones (1, 3) ,  le-3*ones (1, 3) ] ) ; 


%  AHRS  SIGMA  Weights  Initialization 
nx  =  length (x  bar); 
nQ  =  length (diag (Qv) ) ; 
nR  =  length (diag (R) ) ; 

States 

nxa  =  nx+nQ+nR; 

States 
n  =  nxa+2; 
alpha  =  le-3; 

Factor 
beta  =  2; 

PDF 

WxO  =  1/3; 

Wx  =  zeros (n, 1 ) ; 

%  Simplex  Weights 


for  k  =  l:n 

if  k  ==  1 

Wx  ( k) 

=  WxO; 

else 

Wx  ( k) 

=  (1-WxO) / (nxa+1) ; 

end 

%  Number  of  AHRS  States 
%  Process  Noise  States 
%  Measurement  Noise 

%  Total  Augmented 

%  Number  of  Iterations 
%  Tunable  Scaleing 

%  beta  =  2  for  Gaussian 

%  Tunable,  0<W<1 
%  Pre-fill 


end 

%  Scaled  Simplex  Weights 
for  k  =  l:n 
if  k  ==  1 


wx(k)=  1+ (Wx  ( k) -1 ) /alpha''2  ; 

else 

wx(k)  =  Wx ( k) /alpha^2 ; 

end 

end 

%  Incorperate  prior  PDF  knowledge  for  Higher  Order  Moments 
for  k  =  1 : n 
if  k  ==  1 


wm ( k )  =  wx ( k ) ; 

wc(k)  =  wx (k) + (l-alpha^2+beta) ; 

else 

wm ( k )  =  wx ( k ) ; 
wc  ( k)  =  wx  ( k)  ; 

end 

end 


%Sigma  Matrix 

sig  =  zeros (nxa,  n) ; 

sig(l,l)  =  0; 

sig(l,2)  =  -1/sqrt (2*wx (2) ) ; 
sig(l,3)  =  1/sqrt (2*wx (2) ) ; 
for  j  =  2 : nxa 

for  i  =  1:1 

sig(j,i)  =  0; 

end 

for  i  =  2 : j  +1 

sig(j,i)  =  -1/sqrt (j * (j+1) *wx (2)); 


%  Pre-fill 
%  Initialize  (1,1) 
%  Initialize  (1,2) 
%  Initialize  (1,3) 
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end 


end 

for  i  =  j  +2 : j  +2 

sig(j,i)  =  j /sqrt ( j * ( j+1) *wx (2) ) ; 

end 


%  INS  SIGMA  Weights  Initialization 
nx2  =  length (x  bar2); 
nQ2  =  length (diag (Qv2 )) ; 
nR2  =  length (diag (R2 )) ; 

States 

nxa2  =  nx2+nQ2+nR2 ; 

States 

n2  =  nxa2+2; 
alpha  =  le-3; 
beta  =  2; 

PDF 

Wx02  =  1/3; 

Wx2  =  zeros (n2 , 1 ) ; 

%  Sigma  Weights 


k  =  1 : n2 

if  k  ==  1 

Wx2 ( k) 

=  Wx02; 

else 

Wx2 ( k) 

=  (1-Wx02) / (nxa2+l) ; 

end 

end 

%  Scaled  Sigma  Weights 
for  k  =  1 : n2 
if  k  ==  1 

wx2(k)=  1+ (Wx2  ( k) -1 ) /alpha''2  ; 

else 

wx2(k)  =  Wx2  ( k) /alpha''2  ; 

end 

end 

%  Incorperate  prior  PDF  knowledge  for  Higher  Order  Moments 
for  k  =  1 : n2 
if  k  ==  1 

wm2 ( k)  =  wx2 ( k) ; 

wc2(k)  =  wx2 (k) + (l-alpha^2+beta) ; 

else 

wm2 ( k)  =  wx2 ( k) ; 
wc2 ( k)  =  wx2 ( k) ; 

end 

end 

%  Sigma  Matrix 
sig2  =  zeros (nxa2,  n2); 
sig2 (1,1)  =  0 ; 

sig2(l,2)  =  -1/sqrt (2*wx2 (2) ) ; 
sig2(l,3)  =  1/sqrt (2*wx2 (2)); 


%  Pre-fill 
%  Initialize  (1,1) 
%  Initialize  (1,2) 
%  Initialize  (1,3) 


%  Number  of  INS  States 
%  Process  Noise  States 
%  Measurement  Noise 

%  Total  Augmented 

%  Total  Iterations 
%  Tunable,  0<alpha<l 
%  beta  =  2  for  Gaussian 

%  Tunable,  0<W<1 
%  Pre-fill 
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0; 


for  j  =  2 : nxa2 
for  i  =  1:1 

sig2(j,i)  = 

end 

for  i  =  2 : j  +1 

sig2(j,i)  =  -1/sqrt ( j* ( j+1) *wx2 (2) ) ; 

end 

for  i  =  j  +2 : j  +2 

sig2(j,i)  =  j /sqrt ( j * ( j+1) *wx2 (2) ) ; 

end 

end 

end 

%%  AHRS  ESTIMATOR 

%  Build  Augmented  State  Vector 

X  =  zeros (nxa,  n) ; 

xa  =  [x  bar;  zeros (nxa-nx, 1 )] ; 

%  Build  Augmented  Covariance  Matrix 
Pxa  =  [Px  zeros (nx,nQ)  zeros (nx, nR) ; 

zeros (nQ,nx)  Qv  zeros (nQ, nR) ; 
zeros (nR,nx)  zeros (nR,nQ)  R] ; 

%  Build  n+2  Sigma  Vectors 
for  k  =  1 : n 

X(:,k)  =  xa  +  Pxa' *sig  ( : ,  k)  ; 

end 

%  Time  Update 
Xx  =  zeros (nx,  n) ; 

X  bar  =  zeros (nx,  1); 
for  k  =  1 : n 

%  Gyro  Model  w  bi (true)  =  w_bi (measured)  -  bias  -  noise 
w_bi_b  =  [p-X ( 5 , k) -X ( 8 , k) ;  q-X (6, k) -X (9, k) ;  r-X(7,  k)-X(10,k)] 

if  SIM  ==  1 

%  No  Sidereal  Rate 
w  bt  =  w  bi  b; 

else 

%  Sidereal  Rate  Included 
R_t2b  =  rot_t2b (X ( 1 : 4 , k) ) ; 

w_bt  =  w_bi_b  -  R_t2b* [wei*cos (latO) ;  0;  -wei*sin (latO) ] ; 

end 

%  Quaternion  Update 


wl  =  w  bt(l);  w2  =  w  bt(2); 

w3  = 

w  bt  ( 3 

)  ; 

Q_kpl  =  X(l:4,  k)  +  Tdt/2)* 

[0, 

1 

1  ' — 1 

1 

w2 ,  -w3; 

wl , 

0, 

w3. 

CM 

[ 

CM 

-w3. 

0, 

wl ; 

w3. 

w2 , 

1 — 1 

1 

0]*X(1:4,  k); 

%  Quaternion  Normalization 
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Q  kpl  n  =  Q  k;pl/sqrt(Q  k;pl'*Q  kpl); 

%  Gyro  Bias  Update 
if  SIM  ==  1 

%  Constant  Bias 
bg  =  X  ( 5  :  7 ,  k)  ; 

else 

%  Random  Walk  Bias 
bg  =  X(5:7,k)  +  dt*X(ll:13,k); 

end 

%  Rebuild  the  State  Vector  for  Each  Iteration 
Xx(:,k)  =  [Q_kpl_n;  bg] ; 

%  Calculated  the  Weighted  Mean  of  the  State  Vector 
X  bar  =  X  bar  +  wm ( k) *Xx ( : , k) ; 
end 

%  Calculate  Error  Covariance 
Px=  zeros (nx,  nx) ; 
for  k  =  1 : n 

Px  =  Px  +  wc ( k) * ( (Xx ( : ,  k)  -  x  bar ) * (Xx ( : ,  k)  -  x  bar)'); 

end 

%  Non-Linear  Measurement  Process  Equations 
Y  =  zeros (nR,  n) ; 
y_bar  =  zeros (nR, 1 ) ; 

Heading  =  zeros (l,n); 
if  isempty(Hl) 

HI  =  zeros ( 1 , n) ; 

H2  =  zeros ( 1 , n) ; 

Headings  =  zeros (l,n); 
ii  =  zeros ( 1 , n) ; 

end 

for  k  =  1 : n 

%  Rotation  Matrix  (LTP  to  Body) 

R  t2b  =  rot  t2b (Xx ( 1 : 4 , k) ) ; 

%  Gyro  Measurements 
w_bi  =  [p;  q;  r]  -  Xx(5:7,k); 
if  SIM  ==  1 

noise  R  =  X(ll;14,k); 
w  bt  =  w  bi; 

else 

noise  R  =  X(14:17,k); 
w_bt  =  w_bi  -  R_t2b* [wei*cos (latO) ;  0;  -wei*sin (latO) ] ; 

end 

%  Quaternion  to  Euler  Angles 

qO  =  Xx(l,k);  ql  =  Xx(2,k);  q2  =  Xx(3,k);  q3  =  Xx(4,k); 

psi  =  mod (atan2 (2* (ql*q2+q0*q3) ,  q0^2+ql^2-q2^2-q3^2) +  noise  R(4), 

2*pi) ; 

%  Unwrap  Heading  Angle  From  [0,  2*pi]  Range  to  Aviod  Jumps 
if  nn  ==  1 


%  Pre-fill 
%  Pre-fill 
%  Pre-fill 

%  Pre-fill 
%  Pre-fill 
%  Pre-fill 
%  Pre-fill 
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HI (k)  =  psi; 

HeadingO(k)  =  Hl(k); 

Heading (k)  =  Hl(k); 

else 

H2 (k)  =  psi; 

if  (H2 (k) -HI (k) )  <=  -100*pi/180 
ii(k)  =  ii(k)+l; 

end 

if  (H2 (k) -HI (k) )  >=  100*pi/180 
ii (k)  =  ii (k) -1; 

end 

Heading(k)  =  HeadingO (k) + ( (H2 (k) +2*pi*ii (k) ) 
HI (k)  =  H2 (k) ; 

end 

%  Accelerometer  Estimate 

FB_hat  =  R_t2b* [ax_gps;  ay_gps;  az_gps]  +  ... 

cross (w_bt,  R_t2b*x_bar2 (4 : 6) )  -  ... 
R_t2b*[0;  0;  g] -x_bar2 ( 7 : 9 ) -noise_R ( 1 : 3 ) ; 

%  Calculated  Observations 
Y ( ; , k) = [FB_hat;  Heading ( k) ] ; 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm ( k) *Y ( : , k) ; 
end 

%  Estimated  Observation  Covariance 
By  =  zeros (nR, nR) ; 
for  k  =  l:n 

Py  =  Py  +  wc ( k) * ( ( Y ( : , k)  -  y_bar) * (Y ( : , k)  -  y 

end 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx, nR) ; 
for  k  =  l:n 

Pxy  =  Pxy  +  wc ( k) * ( (Xx ( : , k)  -  x  bar) * (Y ( : , k) 

end 

%  Measurement  Method  2  (Accelerometer) 

%  Heading  Unwrap  from  [0,  2*pi] 
if  nn  ==  1 

H3  =  heading; 

H4  =  0; 
psiO  =  H3; 
psi  m  =  H3; 

j  j=0; 

else 

H4  =  heading; 

if  (H4-H3)  <=  -100*pi/180 

jj  =  jj+1; 

end 

if  (H4-H3)  >=  100*pi/180 


-  HeadingO ( k) ) 


bar) ' ) ; 


-  y_bar) ' ) ; 


178 


jj  =  jj-1; 

end 

psi_m  =  psiO+ ( (H4+2*pi* j j )  -  psiO); 

H3  =  H4; 

end 

%  Measurement  Vector 
Z  =  [fx;  fy;  fz;  psi  m]  ; 

%  Kalman  Filter  Equations 
K  =  Pxy/Py; 

X  bar  =  X  bar  +  K* (Z  -  y  bar) ; 

%  Covariance  Corrections 
Px  =  Px  -  K*Py*K'; 

nn  =  2 ; 

%%  INS  ESTIMATOR 

%  Build  Augmented  State  Vector 
X  =  zeros (nxa2,  n2); 
xa  =  [x  bar2;  zeros (nxa2-nx2 , 1 )] ; 

%  Build  Augmented  Covariance  Matrix 
Pxa2  =  [Px2  zeros (nx2 , nQ2 )  zeros (nx2 , nR2 ) ; 

zeros (nQ2 , nx2 )  Qv2  zeros (nQ2 , nR2 ) ; 
zeros (nR2 , nx2 )  zeros (nR2 , nQ2 )  R2 ]  ; 

%  Construct  n+2  SIGMA  Vectors 
for  k  =  1 : n2 

X ( : ,  k)  =  xa  +  Pxa2 '  *sig2 ( : ,  k) ; 

end 

%  Time  Update 
Xx  =  zeros (nx2,  n2); 

X  bar2  =  zeros (nx2,  1); 
for  k  =  1 : n2 
%  Rotation  Matrix 
R  t2b  =  rot  t2b(x  bar (1:4)); 

R  b2t  =  R  t2b' ; 

%  Position  Update 
P_kpl  =  X(l:3,k)  +  dt*[X(4:5,k)  ;  -X(6,k)]; 

%  Velocity  and  Bias  Update 
if  SIM  ==  1 
%  No  Sidereal 

V_kpl  =  X(4:6,k)  +  dt* (R_b2t* ( [fx;  fy;  fz]  -  X(7:9,k)  - 
X(10:12,  k))  +  [0;  0;  g] ) ; 
ba_kpl  =  X(7:9,k); 

else 

%  Sidereal  Included 

V_kpl  =  X(4:6,k)  +  dt* (R_b2t* ( [fx;  fy;  fz]  -  X(7:9,k)  - 


%  Pre-fill 
%  Pre-fill 


%  Kalman  Gain 
%  Kalman  Correction 
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X(10:12,  k)  )  +  [0;  0;  g]  -  2 *cross ( [wei*cos ( latO ) ;  .. 

0;  -wei*sin (latO) ] ,  X(4:6,k))); 
ba_kpl  =  X(7:9,k)  +  dt*X(13:15,k); 

end 

%  States 

Xx ( : , k)  =  [P  kpl;  V  kpl;  ba  kpl]; 

%  Calculated  Mean 

X  bar2  =  x  bar2  +  wm2 ( k) *Xx ( : , k) ; 
end 

%  Predicted  Error  Covariance 
Px2=  zeros (nx2,  nx2); 
for  k  =  1 : n2 

Px2  =  Px2  +  wc2 ( k) * ( (Xx ( : , k)  -  x  bar2 ) * (Xx ( : , k)  -  x  bar2)'); 

end 

%  Measurement  Equations 
Y  =  zeros (nR2,  n2); 
y_bar  =  zeros (nR2 , 1 ) ; 
for  k  =  1 : n2 

if  SIM  ==  1 

%  Calculated  Observations 
Y(:,k)=Xx(l:6,k)+X(13:18,k)  ; 

else 

%  Calculated  Observations 
Y  ( : ,  k)  =Xx  (1 :  6,  k)  +X  ( 1 6  :  2 1 ,  k)  ; 

end 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm2 ( k) *Y ( : , k) ; 
end 

%  Estimated  Observation  Covariance 
Py  =  zeros (nR2 , nR2 ) ; 
for  k  =  1 : n2 

Py  =  Py  +  wc2 ( k) * ( ( Y ( : , k)  -  y_bar) * (Y ( : , k)  -  y_bar) ' ) ; 

end 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx2 , nR2 ) ; 
for  k  =  1 : n2 

Pxy  =  Pxy  +  wc2 ( k) * ( (Xx ( : , k)  -  x  bar2 ) * ( Y ( : , k)  -  y  bar)'); 

end 

%  Measurements 

Z  =  [N_sat;  E_sat;  D_sat;  speed*cos (CoG) ;  speed*sin (CoG)  ;  Vd_sat] 
%  Kalman  Filter  Equations 

K  =  Pxy/Py;  %  Kalman  Gain 

%  Kalman  State  Corrections 


%  Pre-fill 
%  Pre-fill 
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X  bar2  =  x  bar2  +  K* (Z  -  y  bar) ; 

%  Covariance  Correction 
Px2  =  Px2  -  K*Py*K'; 

X  HAT  =  [x  bar (1:4);  x  bar2(l:6);  x  bar (5: 7);  x  bar2(7:9)]; 
end 

kk=kk+l; 

end 

function  R  t2b  =  rot  t2b(x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

R_t2b  =  [q0^2+ql^2-q2^2-q3^2  2* (ql*q2+q0*q3)  2*(ql*q3-q0*q2); 

2*  (q2*ql-q0*q3)  qO ^2 -ql ''2+q2 ^2 -q3 '^2  2*(q2*q3+q0*ql); 

2* (q3*ql  +  q0*q2)  2  * ( q3 *q2 -qO *ql )  qO ^2 -ql ^2 -q2 ^2  +  q3 ^2 ] ; 

end 

function  q  =  e2q(phi,  theta,  psi) 

%  Form  Rotation  Matrix 

R_psi  =  [cos (psi)  sin (psi)  0;  -sin (psi)  cos (psi)  0;  0  0  1]; 

R_theta  =  [cos (theta)  0  -sin (theta) ;  0  1  0;  sin (theta)  0  cos (theta) ] ; 
R_phi  =[10  0;  0  cos (phi)  sin (phi);  0  -sin (phi)  cos  (phi)]; 

R  n2b  =  R  phi*R  theta*R  psi; 

%  Extract  Quaternions 
q0=sqrt (1+trace (R_n2b) ) /2; 
ql= (R_n2b (2,3) -R_n2b (3, 2) ) / (4*q0) ; 
q2= (R_n2b (3,1) -R_n2b (1,3) ) / ( 4 *q0 )  ; 
q3= (R_n2b (1, 2) -R_n2b (2,1))/ (4*q0)  ; 

q  =  [qO;  ql;  q2 ;  q3]; 
end 

F.  SQUARE  ROOT  SPHERICAL  SIMPLEX  UNSCENTED  KALMAN 
FILTER  MATLAB  CODE 

1,  Measurement  Model  One  Implementation 

function  X_HAT  =  SRSSUKFl (u) 

%%  SQUARE  ROOOT  SPHERICAL  SIMPLEX  UNSCENTED  KALMAN  FILTER 
%  Measurement  Model  1 
%  LT  Steven  Terjesen 
%  September  2014 

%  This  estimator  is  built  for  use  in  MATLAB  Simulink.  There  are  38 
inputs 

%  required  and  can  be  run  in  Simulink  with  an  "Interpolated  MATLAB 
%  Function"  block.  The  first  13  inputs  are  the  vehicle  data:  Course 
Over 

%  Ground,  Speed  Over  Ground,  Accelerometer  Measurements,  Gyro 
Measurements , 

%  GPS  Measurements  (In  LTP  XNorth-YEast-ZDown) ,  Heading  Measurement, 
and 
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%  Vertical  Velocity  (zeroed  out  for  SEAFOXII  data) .  Inputs  14  through 
34 

%  are  the  Process  Noise  and  Measurement  Noise  diagonal  elements.  These 
%  elements  are  not  hard  coded  to  allow  for  easier  tuning.  Inputs  35 
%  through  37  are  the  N-E-D  accelerations  in  the  LTP  frame.  Input  38  is 
a 

%  toggle  for  selecting  whether  the  data  is  from  Condor  or  SEAFOX  II. 

%%  System  Inputs 
%  Measurement  inputs 


CoG  =  u ( 1 ) ; 

%GPS 

Course  Over  Ground 

speed  =  u(2); 

%GPS 

Speed  Over  Ground 

fx  =  u  ( 3 )  ;  f  y 

=  u(4) 

;  f  z  =  u  ( 5 )  ; 

%IMU 

Accelerations 

p  =  u  (  6 )  ;  q  = 

u(7)  ; 

r  =  u  ( 8 )  ; 

%IMU 

Angular  Rates 

N  sat  =  u ( 9) ; 

E  sat 

=  u(10);  D  sat  =  u(ll); 

%GPS 

Position  (NED) 

heading  =  u ( 12 ) ; 

%GPS 

Heading 

Vd  sat  =  u(13) 

'  ; 

%GPS 

LTP  Down  Velocity 

%  Process  and  Measurement  Noise  Mat 
R  =  chol (diag (u (14 : 16) ) )  ; 

R2  =  chol (diag (u  ( 17  :  22 ) ) )  ; 

SIM  =  u  (38) ; 

SEAFOXII (2) 

if  SIM  ==  1 

Qv  =  chol (diag (u (23 : 25) ) )  ; 
Condor 

Qv2  =  chol (diag (u (2 9 : 31 ) ) ) ; 
Condor 
else 

Qv  =  chol (diag (u (23 : 2 8 ) ) ) ; 
SEAFOX 

Qv2  =  chol (diag (u (2 9 : 34 ) ) ) ; 
SEAFOX 

Vd_sat  =  0; 

0, 

o 

on 

q, 

o 

D_sat  =  0; 

Measurement 

q, 

o 

set 

q, 

o 

end 

%  GPS  Accelerations 

ax_gps  =  u(35);  ay_gps  =  u(36);  az_ 


ices 

%AHRS  Measurement  Noise 
%INS  Measurement  Noise 

%Condor(l)  or 


%AHRS  Process  Noise 
%INS  Process  Noise 


%AHRS  Process  Noise 

%INS  Process  Noise 

%No  Vertical  Velocity 
measurement  available 

SEAFOX  II,  set  Vd=0. 
%No  Altitude 

available  on  SEAFOXII, 

D  sat  =  0; 


=  u(37);  %GPS  Accelerations  as 
%  calculated  from  3rd 
%  Order  Filter 


%%  Initialization 

persistent  x  bar  HeadingO  HI  H2  ii  j j  psiO  H3  H4  . . . 
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Sx  Sx2  kk  dt  g  nn  nx  nxa  nQ  nR  n  wc  wm  x  bar2  nx2  nxa2  n2  nQ2  nR2  wc2 
wm2  sig  sig2  wei  latO 

%  Allows  time  for  Condor  To  steady  out  during  live  testing 
if  isempty(kk) 
kk  =  0; 

end 

if  kk  <  1  &&  SIM==1 

X  HAT  =  zeros (16,  1)  ; 

else 

if  isempty(x  bar) 

%  Miscellaneous 
dt  =  0.01; 
g  =  9.8; 

[m/s^2 ] 

wei  =  7.292115*10^-5; 
lat0=  0.639268394832413; 

%lon0  =  -2.115435878466264 
nn=l ; 

Initializer 

%  AHRS  Initialization 
%  Euler  Angle  Initialization 

rx  =  ax  gps*cos (heading) +ay  gps*sin (heading) ; 
ry  =  -ax_gps*sin (heading) +ay_gps*cos (heading) ; 
rz  =  az_gps-g; 

theta  0  =  atan((-rx*rz  -  fx*sqrt (rx^2  +  rz^2-fx^2) ) / (fx^2-rz^2) )  ; 
r_theta  =  rx*sin (theta_0)  +  rz*cos (theta_0) ; 
fc  =  f y- speed* r; 

phi  0  =  atan((r  theta*ry  +  fc*sqrt (ry^2+r  theta^2-fc^2) ) / (fc^2- 
r_theta''2 )  )  ; 
psi  0=heading; 

%  Euler  Angles  to  Quaternions 
q_0  =  e2q(phi_0,  theta_0,  psi_0); 

%  Gyro  Bias  Initial  Values 
bg_0  =  [0;  0;  0]  ; 

%  Inital  State  Vector 
x_bar  =  [q_0;  bg_0] ; 

%  Initial  Covariance  Estimate 
Px  =  diag ( [le-6*ones (1, 4)  le-3*ones (1, 3) ] ) ; 

%  Inital  Matrix  Square  Root 
Sx  =  chol ( Px)  ; 

%INS  Initialization 

%  Position  &  Velocity  Initialization 
x_0  =  N_sat; 
y_0  =  E_sat; 
z_0  =  D_sat; 
vn_0  =  speed*cos (CoG) ; 
ve_0  =  speed*sin (CoG) ; 
vd_0  =  Vd_sat; 

%  Accelerometer  Initial  Bias  Estimate 
ba  0  =  [ 0 ;  0 ;  0 ] ; 


%Filter  dt  [sec] 
%Gravity  Constant 

%Sidereal  Rate  [rad/s] 
%Origing  of  LTP  [rad] 

%Heading  Count 
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%  Initial  State  Vector 

x_bar2  =  [x_0;  y_0;  z_0;  vn_0;  ve_0;  vd_0;  ba_0] ; 

%  Initial  Covariance  Estimate 

Px2  =  diag ( [le-5*ones (1, 3) ,  le-5*ones (1, 3) ,  le-3*ones (1, 3) ] ) ; 
%  Inital  Matrix  Square  Root 
Sx2  =  chol(Px2); 


%  AHRS  SIGMA  Weights  Initialization 
nx  =  length (x  bar); 
nQ  =  length (diag (Qv) ) ; 
nR  =  length (diag (R) ) ; 

States 

nxa  =  nx+nQ+nR; 

States 
n  =  nxa+2; 
alpha  =  le-3; 

Factor 
beta  =  2; 

PDF 

WxO  =  1/3; 

Wx  =  zeros (n, 1 ) ; 

%  Simplex  Weights 
for  k  =  1 : n 
if  k  ==  1 

Wx(k)  =  WxO; 

else 

Wx(k)  =  (1-WxO) / (nxa+1) ; 

end 


%  Number  of  AHRS  States 
%  Process  Noise  States 
%  Measurement  Noise 

%  Total  Augmented 

%  Number  of  Iterations 
%  Tunable  Scaling 

%  beta  =  2  for  Gaussian 

%  Tunable,  0<W<1 
%  Pre-fill 


end 

%  Scaled  Simplex  Weights 
for  k  =  1 : n 
if  k  ==  1 


wx(k)=  1+ (Wx ( k) -1 ) /alpha^2 ; 

else 

wx(k)  =  Wx ( k) /alpha^2 ; 

end 


end 

%  Incorperate  prior  PDF  knowledge  for  Higher 
for  k  =  1 : n 
if  k  ==  1 


wm  ( k) 
wc  ( k) 

else 

wm  ( k) 
wc  ( k) 

end 


end 


wx ( k ) ; 

wx  (k)  +  ( l-alpha''2+beta)  ; 

wx ( k ) ; 
wx ( k ) ; 


Order  Moments 


%Sigma  Matrix 

sig  =  zeros (nxa,  n) ; 

sig(l,l)  =  0; 

sig(l,2)  =  -1/sqrt (2*wx (2) ) ; 
sig(l,3)  =  1/sqrt (2*wx (2) ) ; 
for  j  =  2 : nxa 

for  i  =  1 : 1 


%  Pre-fill 
%  Initialize  (1,1) 
%  Initialize  (1,2) 
%  Initialize  (1,3) 
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sig(j,i)  =  0; 

end 

for  i  =  2 : j  +1 

sig(j,i)  =  -1/sqrt ( j * ( j+1) *wx (2) ) ; 

end 

for  i  =  j  +2 : j  +2 

sig(j,i)  =  j /sqrt ( j * ( j+1) *wx (2) ) ; 

end 


end 


%  INS  SIGMA  Weights  Initialization 
nx2  =  length (x  bar2); 
nQ2  =  length (diag (Qv2 )) ; 
nR2  =  length (diag (R2 )) ; 

States 

nxa2  =  nx2+nQ2+nR2 ; 

States 

n2  =  nxa2+2; 
alpha  =  le-3; 
beta  =  2; 

PDF 

Wx02  =  1/3; 

Wx2  =  zeros (n2 , 1 ) ; 


%  Number  of  INS  States 
%  Process  Noise  States 
%  Measurement  Noise 

%  Total  Augmented 

%  Total  Iterations 
%  Tunable,  0<alpha<l 
%  beta  =  2  for  Gaussian 

%  Tunable,  0<W<1 
%  Pre-fill 


%  Sigma  Weights 
for  k  =  1 : n2 
if  k  ==  1 

Wx2 ( k)  =  Wx02 ; 

else 

Wx2(k)  =  ( 1-Wx02 ) / (nxa2+l ) ; 

end 

end 

%  Scaled  Sigma  Weights 
for  k  =  1 : n2 
if  k  ==  1 

wx2(k)=  1+ (Wx2  ( k) -1 ) /alpha''2  ; 

else 

wx2(k)  =  Wx2 ( k) /alpha^2  ; 

end 

end 

%  Incorperate  prior  PDF  knowledge  for  Higher  Order  Moments 
for  k  =  1 : n2 
if  k  ==  1 

wm2 ( k)  =  wx2 ( k) ; 

wc2(k)  =  wx2  (k)  +  (l-alpha''2+beta)  ; 

else 

wm2 ( k)  =  wx2 ( k) ; 
wc2 ( k)  =  wx2 ( k) ; 

end 

end 


%  Sigma  Matrix 
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sig2  =  zeros (nxa2,  n2);  % 

sig2 ( 1 , 1 )  =  0 ;  % 

sig2(l,2)  =  -1/sqrt (2*wx2 (2) ) ;  % 

sig2(l,3)  =  1/sqrt (2*wx2 (2) ) ;  % 

for  j  =  2 : nxa2 
for  i  =  1 : 1 

sig2 ( j , i)  =  0; 

end 

for  i  =  2 : j  +1 

sig2(j,i)  =  -1/sqrt (j * (j+1) *wx2 (2) ) ; 

end 

for  i  =  j  +2 : j  +2 

sig2(j,i)  =  j /sqrt ( j * ( j+1) *wx2 (2) ) ; 

end 

end 

end 

%%  AHRS  ESTIMATOR 

%  Build  Augmented  State  Vector 

X  =  zeros (nxa,  n) ; 

xa  =  [x  bar;  zeros (nxa-nx, 1 )] ; 

%  Build  Augmented  Covariance  Matrix 
Sxa  =  [Sx  zeros (nx,nQ)  zeros (nx, nR) ; 

zeros (nQ,nx)  Qv  zeros (nQ, nR) ; 
zeros (nR,nx)  zeros (nR,nQ)  R]  ; 

%  Build  n+2  Sigma  Vectors 
for  k  =  1 : n 

X ( : ,  k)  =  xa  +  Sxa*sig ( : , k) ; 

end 

%  Time  Update 
Xx  =  zeros (nx,  n) ; 

X  bar  =  zeros (nx,  1); 
for  k  =  1 : n 

%  Gyro  Model  w  bi (true)  =  w_bi (measured)  -  bias 
w_bi_b  =  [p-X ( 5 , k) -X ( 8 , k) ;  q-X ( 6, k) -X ( 9, k) ;  r-X(7 

if  SIM  ==  1 

%  No  Sidereal  Rate 
w  bt  =  w  bi  b; 

else 

%  Sidereal  Rate  Included 
R_t2b  =  rot_t2b (X ( 1 : 4 , k) ) ; 

w  bt  =  w  bi  b  -  R  t2b* [wei*cos (latO) ;  0;  -wei 

end 

%  Quaternion  Update 

wl  =  w  bt(l);  w2  =  w  bt(2);  w3  =  w  bt(3); 

Q  kpl  =  X(l:4,  k)  +  (dt/2)*[0,  -wl,  -w2,  -w3; 

wl,  0,  w3,  -w2; 
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Pre-fill 
Initialize  (1,1) 
Initialize  (1,2) 
Initialize  (1,3) 


noise 

k)-X(10,k)]; 


sin ( latO ) ] ; 


w2 ,  -w3,  0,  wl; 

w3,  w2 ,  -wl,  0]*X(1:4,  k) ; 

%  Quaternion  Normalization 
Q  kpl  n  =  Q  kpl/sqrt(Q  kpl'*Q  kpl); 

%  Gyro  Bias  Update 
if  SIM  ==  1 

%  Constant  Bias 
bg  =  X  ( 5  :  7 ,  k)  ; 

else 

%  Random  Walk  Bias 
bg  =  X(5:7,k)  +  dt*X(ll:13,k); 

end 

%  Rebuild  the  State  Vector  for  Each  Iteration 
Xx(:,k)  =  [Q_kpl_n;  bg] ; 

%  Calculated  the  Weighted  Mean  of  the  State  Vector 
X  bar  =  X  bar  +  wm ( k) *Xx ( : , k) ; 
end 

%  Calculate  Error  Covariance 

Ax  =  sqrt (wc (2 ) ) * (Xx ( : , 2 : n)  -  x_bar ( : , ones ( 1 , n-1 ) ) ) ; 

%  QR  Decomposision 
[~,  sx]  =  qr (Ax' , 0) ; 

%  Cholesky  Update  (Downdate  for  negative  zeroth  weight) 

Sx  =  cholupdate ( sx,  sqrt ( -wc ( 1 ) ) * (Xx ( : , 1 ) -x_bar ) , 

%  Non-Linear  Measurement  Process  Equations 
Y  =  zeros (nR,  n) ; 
y_bar  =  zeros (nR, 1 ) ; 

Heading  =  zeros (l,n); 
if  isempty(Hl) 

HI  =  zeros ( 1 , n) ; 

H2  =  zeros ( 1 , n) ; 

Headings  =  zeros (l,n); 
ii  =  zeros ( 1 , n) ; 

end 

for  k  =  1 : n 

if  SIM  ==  1 

noise  R  =  X(ll:13,k); 

else 

noise  R  =  X(14:16,k); 

end 

%  Quaternion  to  Euler  Angles 
qO  =  Xx(l,k);  ql  =  Xx(2,k);  q2  =  Xx(3,k);  q3  =  Xx(4,k); 
phi  =  atan2 (2* (q2*q3+q0*ql ) ,  q0^2-ql^2-q2^2+q3^2)  +  noise  R ( 1 ) ; 
theta  =  asin (-2* (ql*q3-q0*q2) ) +  noise_R(2); 

psi  =  mod  (  (atan2  (2*  (ql*q2+q0*q3)  ,  q0''2+ql''2-q2''2-q3''2)  )  +  noise  R(3), 
2*pi) ; 


%  Pre-fill 
%  Pre-fill 
%  Pre-fill 

%  Pre-fill 
%  Pre-fill 
%  Pre-fill 
%  Pre-fill 
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%  Unwrap  Heading  Angle  From  [0,  2*pi]  Range  to  Aviod  Jumps 
if  nn  ==  1 

HI (k)  =  psi; 

HeadingO(k)  =  Hl(k); 

Heading (k)  =  Hl(k); 

else 

H2 (k)  =  psi; 

if  (H2 (k) -HI (k) )  <=  -100*pi/180 
ii(k)  =  ii(k)+l; 

end 

if  (H2 (k) -HI (k) )  >=  100*pi/180 
ii (k)  =  ii (k) -1; 

end 

Heading (k)  =  HeadingO (k) + ( (H2 (k) +2*pi*ii (k) )  -  HeadingO ( k) ) ; 

HI (k)  =  H2 (k) ; 

end 

%  Calculated  Observations 
Y ( : , k) = [phi ;  theta;  Heading ( k)]; 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm ( k) *Y ( ; , k) ; 
end 

%  Estimated  Observation  Covariance 

Bx  =  sqrt (wc (2 ) ) * (Y ( ;  ,  2 : n)  -  y_bar ( : , ones ( 1 , n-1 ) ) ) ; 

%  QR  Decomposition 

[~,  sy]  =  qr(Bx',0);  %  **"qr"  PRODUCES  UPPER  TRIANGULAR  MATRIX** 
%  Cholesky  Update  (or  Downdate) 

Sy  =  cholupdate ( sy,  sqrt ( -wc ( 1 ) ) * ( Y ( : , I ) -y_bar ) , 

%  ***Sy  MUST  BE  LOWER  TRIANGULAR*** 

Sy  =  Sy' ; 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx,  nR)  ; 
for  k  =  1 : n 

Pxy  =  Pxy  +  wc ( k) * ( (Xx ( : , k)  -  x_bar) * (Y ( : , k)  -  y_bar) ' ) ; 

end 

%  Measurement  Method  1  (Accelerometer-GPS-Compass  Fusion) 

%  Rotate  GPS  XYZ  Acceration  about  Heading 
rx  =  ax  gps*cos (heading) +ay  gps*sin (heading) ; 
ry  =  -ax  gps*sin (heading) +ay  gps*cos (heading) ; 
rz  =  az_gps-g; 

%  Computer  Pitch 

theta  =  atan((-rx*rz  -  fx*sqrt  (rx^2+rz''2-fx^2)  )  /  (fx^2-rz''2)  )  ; 

%  Compute  Roll 

r_theta  =  rx*sin (theta)  +  rz*cos (theta) ; 

fc  =  fy-(norm(x  bar2 (4 : 6) ) ) *r;  %  Added  Coriolis  Term 

phi  =  atan((r  theta*ry  +  fc*sqrt (ry^2+r  theta^2-fc^2 ) )  /  (fc^2- 

r_theta''2 )  )  ; 

%  Heading  Unwrap  from  [0,  2*pi] 
if  nn  ==  1 
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H3  =  heading; 

H4  =  0; 
psiO  =  H3; 
psi  m  =  H3; 

j  j=0; 

else 

H4  =  heading; 

if  (H4-H3)  <=  -100*pi/180 

jj  =  jj+1; 

end 

if  (H4-H3)  >=  100*pi/180 

jj  =  jj-1; 

end 

psi_m  =  psiO+ ( (H4+2*pi* j j )  -  psiO); 

H3  =  H4; 

end 

%  Measurement  Vector 
Z  =  [phi;  theta;  psi_m] ; 

%  Kalman  Filter  Equations 
K  =  (Pxy/Sy' ) /Sy; 

Squares 

X  bar  =  X  bar  +  K* (Z  -  y  bar) ; 

%  Covariance  Corrections 
Ux  =  K*Sy; 
for  k  =  1 : nR 

Sx  =  cholupdate ( Sx,  Ux ( : ,  k)  , 

end 

Sx  =  Sx' ; 

TRIANGULAR 

nn  =  2 ; 

%%  INS  ESTIMATOR 

%  Build  Augmented  State  Vector 

X  =  zeros (nxa2,  n2); 

xa  =  [x  bar2;  zeros (nxa2-nx2 , 1 )] ; 

%  Build  Augmented  Covariance  Matrix 

Sxa2  =  [Sx2  zeros (nx2 , nQ2 )  zeros (nx2 , nR2 ) ; 

zeros (nQ2 , nx2 )  Qv2  zeros (nQ2 , nR2 ) ; 
zeros (nR2 , nx2 )  zeros (nR2 , nQ2 )  R2 ]  ; 

%  Construct  n+2  SIGMA  Vectors 
for  k  =  1 : n2 

X(:,k)  =  xa  +  Sxa2*sig2 ( : ,  k) ; 

end 

%  Time  Update 

Xx  =  zeros (nx2,  n2); 

X  bar2  =  zeros (nx2,  1); 
for  k  =  1 : n2 


%  Efficient  Least 
%  Kalman  Correction 

%  Cholesky  Downdate 
%  MUST  BE  LOWER 


%  Pre-fill 
%  Pre-fill 
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%  Rotation  Matrix 
R  t2b  =  rot  t2b(x  bar (1:4)); 

R  b2t  =  R  t2b' ; 

%  Posittion  Update 

P_k;pl  =  X(l:3,k;)  +  dt*  [X  ( 4  :  5 ,  k)  ;  -X(6,k)]; 

%  Velocity  and  Bias  Update 
if  SIM  ==  1 
%  No  Sidereal 

V_kpl  =  X(4:6,k)  +  dt* (R_b2t* ( [fx;  fy;  fz]  -  X(7:9,k)  -  .. 

X(10:12,  k))  +  [0;  0;  g] ) ; 
ba_kpl  =  X(7:9,k); 

else 

%  Sidereal  Included 

V_kpl  =  X(4:6,k)  +  dt* (R_b2t* ( [fx;  fy;  fz]  -  X(7:9,k)  -  .. 

X(10:12,  k) )  +  [0;  0;  g]  -  2 *cross ( [wei*cos ( latO ) ; 
0;  -wei*sin (latO) ]  ,  X(4:6,k))); 
ba_kpl  =  X(7:9,k)  +  dt*X(13:15,k); 

end 

%  States 

Xx ( : , k)  =  [P  kpl;  V  kpl;  ba  kpl]; 

%  Calculated  Mean 

X  bar2  =  x  bar2  +  wm2 ( k) *Xx ( : , k) ; 
end 

%  Predicted  Error  Covariance 

Ax  =  sqrt (wc2 (2 ) ) * (Xx ( : , 2 : n2 )  -  x  bar2 ( : , ones ( 1 ,  n2-l ) ) ) ; 

%  QR  Decomposition 
[~,  Sx2]  =  qr  (Ax'  ,  0)  ; 

%  Cholesky  Factor  Update 

Sx2  =  cholupdate ( Sx2  ,  sqrt ( -wc2 ( 1 ) ) * (Xx ( : , 1 ) -x_bar2 ) ,  '-'); 

%  Measurement  Equations 
Y  =  zeros (nR2,  n2); 
y_bar  =  zeros (nR2 , 1 ) ; 
for  k  =  1 : n2 

if  SIM  ==  1 

%  Calculated  Observations 
Y(:,k)=Xx(l:6,k)+X(13:18,k)  ; 

else 

%  Calculated  Observations 
Y(:,k)=Xx(l:6,k)+X(16:21,k)  ; 

end 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm2 ( k) *Y ( : , k) ; 
end 

%  Estimated  Observation  Covariance 

Bx  =  sqrt (wc2 (2 ) ) * (Y ( :  ,  2 : n2 )  -  y_bar ( : , ones ( 1 , n2-l ) ) ) ; 
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%  Pre-fill 
%  Pre-fill 


%  QR  Decomposition 
[~,  Sy]  =  qr (Bx' , 0) ; 

%  Cholesky  Factor  Update 

Sy  =  cholupdate ( Sy,  sqrt ( -wc2 ( 1 ) ) * ( Y ( : , 1 ) -y_bar ) , 

%  Make  Sy  Lower  Triangular 
Sy  =  Sy' ; 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx2 , nR2 ) ; 
for  k  =  1 : n2 

Pxy  =  Pxy  +  wc2 ( k) * ( (Xx ( : , k)  -  x  bar2 ) * ( Y ( : , k)  -  y  bar)'); 

end 

%  Measurements 

Z  =  [N_sat;  E_sat;  D_sat;  speed*cos (CoG) ;  speed*sin (CoG) ;  Vd_sat] ; 

%  Kalman  Filter  Equations 
%  Kalman  Gain  w/  Efficient  Least  Squares 
K  =  (Pxy/Sy' ) /Sy; 

%  Kalman  State  Corrections 
X  bar2  =  x  bar2  +  K* (Z  -  y  bar) ; 

%  Covariance  Correction 
Ux  =  K*Sy; 
for  k  =  1 : nR2 

Sx2  =  cholupdate ( Sx2 ,  Ux ( : , k) ,  '-' ) ; 

end 

%  Make  Sx2  Lower  Right  Trangular 
Sx2  =  Sx2 ' ; 

X  HAT  =  [x  bar (1:4);  x  bar2(l:6);  x  bar (5: 7);  x  bar2(7:9)]; 
end 

kk=kk+l; 

end 

function  R  t2b  =  rot  t2b(x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

R_t2b  =  [q0^2+ql^2-q2^2-q3^2  2* (ql*q2+q0*q3)  2*(ql*q3-q0*q2); 

2* (q2*ql-q0*q3)  qO ^2 -ql ^2+q2 ^2 -q3 ^2  2*(q2*q3+q0*ql); 

2*  (q3*ql  +  q0*q2)  2  *  ( q3  *q2 -qO  *ql )  qO  ^^2 -ql ''2 -q2 ''2  +  q3 ''2  ]  ; 

end 

function  q  =  e2q(phi,  theta,  psi) 

%  Form  Rotation  Matrix 

R_psi  =  [cos (psi)  sin (psi)  0;  -sin (psi)  cos (psi)  0;  0  0  1]; 

R_theta  =  [cos (theta)  0  -sin  (theta);  0  1  0;  sin  (theta)  0  cos (theta) ] 
R_phi  =[10  0;  0  cos (phi)  sin (phi);  0  -sin (phi)  cos  (phi)]; 

R  n2b  =  R  phi*R  theta*R  psi; 

%  Extract  Quaternions 
q0=sqrt (1+trace (R_n2b) ) /2; 
ql= (R_n2b (2,3) -R_n2b (3, 2) ) / (4*q0) ; 
q2= (R_n2b (3,1) -R_n2b (1,3) ) / ( 4  *q0 ) ; 
q3= (R_n2b (1, 2) -R_n2b (2,1))/ (4*q0)  ; 
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q  =  [qO;  ql;  q2 ;  q3]; 
end 


2,  Measurement  Model  Two  Implementation 

function  X_HAT  =  SRSSUKF2 (u) 

%%  SQUARE  ROOOT  SPHERICAL  SIMPLEX  UNSCENTED  KALMAN  FILTER 
%  Measurement  Model  2 
%  LT  Steven  Terjesen 
%  September  2014 

%  This  estimator  is  built  for  use  in  MATLAB  Simulink.  There  are  39 
inputs 

%  required  and  can  be  run  in  Simulink  with  an  "Interpolated  MATLAB 
%  Function"  block.  The  first  13  inputs  are  the  vehicle  data:  Course 
Over 

%  Ground,  Speed  Over  Ground,  Accelerometer  Measurements,  Gyro 
Measurements , 

%  GPS  Measurements  (In  LTP  XNorth-YEast-ZDown)  ,  Heading  Measurement, 
and 

%  Vertical  Velocity  (zeroed  out  for  SEAFOXII  data) .  Inputs  14  through 
35 

%  are  the  Process  Noise  and  Measurement  Noise  diagonal  elements.  These 
%  elementes  are  not  hard  coded  to  allow  for  easier  tuning.  Inputs  36 
%  through  38  are  the  N-E-D  accelerations  in  the  LTP  frame.  Input  39  is 
a 

%  toggle  for  selecting  whether  the  data  is  from  Condor  or  SEAFOX  II. 


%%  System  Inputs 
%  Measurement  inputs 


CoG  =  u ( 1 ) ; 

%GPS 

Course  Over  Ground 

speed  =  u(2); 

%GPS 

Speed  Over  Ground 

fx  =  u (3) ;  fy 

=  u(4) 

;  f  z  =  u  ( 5 )  ; 

%IMU 

Accelerations 

p  =  u  ( 6 )  ;  q  = 

u(7)  ; 

r  =  u  ( 8 )  ; 

%IMU 

Angular  Rates 

N  sat  =  u ( 9) ; 

E  sat 

=  u(10);  D  sat  =  u(ll); 

%GPS 

Position  (NED) 

heading  =  u ( 12 

) ; 

%GPS 

Heading 

Vd  sat  =  u(13) 

r 

%GPS 

LTP  Down  Velocity 

%  Process  and  Measurement  Noise  Matrices 

R  =  chol (diag (u ( 14 : 17 ) ) )  ;  %AHRS  Measurement  Noise 

R2  =  chol (diag (u ( 1 8 : 23 ) ) ) ;  %INS  Measurement  Noise 

%Condor(l)  or 

Simulation  Selector 


SIM  =  u (39) ; 
SEAFOXII (2) 

g, 

o 


if  SIM  ==  1 

Qv  =  chol (diag (u (24 : 26) ) ) ;  %AHRS  Process  Noise 

Condor 

Qv2  =  chol (diag (u (30 : 32) ) ) ;  %INS  Process  Noise 

Condor 
else 

Qv  =  chol (diag (u (24 : 29) ) )  ;  %AHRS  Process  Noise 

SEAFOX 
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Qv2  =  chol (diag (u (30 : 35) ) ) ; 
SEAFOX 

Vd_sat  =  0; 

q, 

o 


on 

q, 

o 


D_sat  = 
Measurement 

q, 

o 


0; 


set 

q, 

o 

end 


%INS  Process  Noise 

%No  Vertical  Velocity 
measurement  available 

SEAFOX  II,  set  Vd=0. 
%No  Altitude 

available  on  SEAFOXII, 

D  sat  =  0; 


%  GPS  Accelerations 

ax_gps  =  u(36);  ay_gps  =  u(37);  az_gps  =  u(38);  %GPS  Accelerations  as 

%  calculated  from  3rd 
%  Order  Filter 


%%  Initialization 

persistent  x  bar  HeadingO  HI  H2  ii  j j  psiO  H3  H4  ... 

Sx  Sx2  kk  dt  g  nn  nx  nxa  nQ  nR  n  wc  wm  x  bar2  nx2  nxa2  n2  nQ2  nR2  wc2 
wm2  sig  sig2  wei  latO 

%  Allows  time  for  Condor  To  steady  out  during  live  testing 
if  isempty(kk) 
kk  =  0; 

end 

if  kk  <  1  &&  SIM==1 

X_HAT  =  zeros (16,1); 

else 

if  isempty(x  bar) 

%  Miscellaneous 
dt  =  0.01; 
g  =  9.8; 

[m/s''2  ] 

wei  =  7.292115*10^-5; 
lat0=  0.639268394832413; 

%lon0  =  -2.115435878466264 
nn=l ; 

Initializer 

%  AHRS  Initialization 
%  Euler  Angle  Initialization 

rx  =  ax  gps*cos (heading) +ay  gps*sin (heading) ; 
ry  =  -ax_gps*sin (heading) +ay_gps*cos (heading)  ; 
rz  =  az_gps-g; 

theta  0  =  atan((-rx*rz  -  fx*sqrt  (rx''2+rz''2-fx''2)  )  /  (fx^2-rz''2)  )  ; 
r_theta  =  rx*sin (theta_0)  +  rz*cos (theta_0) ; 
fc  =  f y- speed* r; 

phi  0  =  atan((r  theta*ry  +  fc*sqrt (ry^2+r  theta^2-fc^2) ) / (fc^2- 
r_theta^2 ) ) ; 
psi  0=heading; 


%Filter  dt  [sec] 
%Gravity  Constant 

%Sidereal  Rate  [rad/s] 
%Origin  of  LTP  [rad] 

%Heading  Count 
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%  Euler  Angles  to  Quaternions 
q_0  =  e2q(phi_0,  theta_0,  psi_0); 

%  Gyro  Bias  Initial  Values 
%  bg_0  =  [2.4e-4;  -le-4;  2e-4]; 
bg_0  =  [ 0 ;  0 ;  0 ]  ; 

%  Inital  State  Vector 
x_bar  =  [q_0;  bg_0] ; 

%  Initial  Covariance  Estimate 

Px  =  diag ( [ le-5*ones ( 1 , 4 )  le-3*ones (1, 3) ] ) ; 

%  Px  =  diag ( [le-5*ones (1, 4)  le-6*ones (1, 3) ] ) ; 
%  Inital  Matrix  Square  Root 
Sx  =  chol ( Px) ; 

%INS  Initialization 

%  Position  &  Velocity  Initialization 


X 

1 

o 

II 

N  sat; 

y_0  = 

E  sat; 

z  0  = 

D  sat; 

vn  0 

=  speed*cos 

(CoG) ; 

ve  0 

=  speed*sin 

(CoG) ; 

vd_0 

=  Vd  sat; 

%  Accelerometer  Initial  Bias  Estimate 
ba_0  =  [ 0 ;  0 ;  0 ]  ; 

%  Initial  State  Vector 

x_bar2  =  [x_0;  y_0;  z_0;  vn_0;  ve_0;  vd_0;  ba_0] ; 

%  Initial  Covariance  Estimate 

Px2  =  diag ( [le-5*ones (1, 3) ,  le-5*ones (1, 3) ,  le-3*ones (1, 3) ] ) ; 
%  Inital  Matrix  Square  Root 
Sx2  =  chol(Px2); 


%  AHRS  SIGMA  Weights  Initialization 
nx  =  length (x  bar); 
nQ  =  length (diag (Qv) ) ; 
nR  =  length (diag (R) ) ; 

States 

nxa  =  nx+nQ+nR; 

States 
n  =  nxa+2; 
alpha  =  le-3; 

Factor 
beta  =  2; 

PDF 

WxO  =  1/3; 

Wx  =  zeros (n, 1 ) ; 

%  Simplex  Weights 


II 

if  k  ==  1 

Wx  ( k) 

=  WxO; 

else 

Wx  ( k) 

=  (1-WxO) / (nxa+1) ; 

end 

end 

%  Scaled  Simplex  Weights 
for  k  =  1 : n 
if  k  ==  1 


%  Number  of  AHRS  States 
%  Process  Noise  States 
%  Measurement  Noise 

%  Total  Augmented 

%  Number  of  Iterations 
%  Tunable  Scaleing 

%  beta  =  2  for  Gaussian 

%  Tunable,  0<W<1 
%  Pre-fill 
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wx(k;)=  1+ (Wx  ( k) -1 ) /alpha^2  ; 

else 

wx(k)  =  Wx  ( k) /alpha''2  ; 

end 

end 

%  Incorperate  prior  PDF  knowledge  for  Higher  Order  Moments 
for  k  =  l:n 
if  k  ==  1 


wm ( k )  =  wx ( k ) ; 

wc(k)  =  wx  (k)  +  (l-alpha''2+beta)  ; 

else 

wm ( k )  =  wx ( k ) ; 
wc  ( k)  =  wx  ( k)  ; 

end 

end 


%Sigma  Matrix 

sig  =  zeros (nxa,  n) ; 

sig(l,l)  =  0; 

sig(l,2)  =  -1/sqrt (2*wx (2) ) ; 
sig(l,3)  =  1/sqrt (2*wx (2) ) ; 
for  j  =  2 : nxa 

for  i  =  1 : 1 

sig(j,i)  =  0; 

end 

for  i  =  2 : j  +1 

sig(j,i)  =  -1/sqrt (j * (j+1) *wx (2) ) ; 

end 

for  i  =  j  +2 : j  +2 

sig(j,i)  =  j /sqrt ( j * ( j+1) *wx (2) ) ; 

end 

end 


%  Pre-fill 
%  Initialize  (1,1) 
%  Initialize  (1,2) 
%  Initialize  (1,3) 


%  Number  of  INS  States 
%  Process  Noise  States 
%  Measurement  Noise 

%  Total  Augmented 

%  Total  Iterations 
%  Tunable,  0<alpha<l 
%  beta  =  2  for  Gaussian 

%  Tunable,  0<W<1 
%  Pre-fill 

%  Sigma  Weights 


k  =  1 : n2 

if  k  ==  1 

Wx2 ( k) 

=  Wx02; 

else 

Wx2 ( k) 

=  (1-Wx02) / (nxa2+l) ; 

end 

end 


%  INS  SIGMA  Weights  Initialization 
nx2  =  length (x  bar2); 
nQ2  =  length (diag (Qv2 )) ; 
nR2  =  length (diag (R2 )) ; 

States 

nxa2  =  nx2+nQ2+nR2 ; 

States 

n2  =  nxa2+2; 
alpha  =  le-3; 
beta  =  2; 

PDF 

Wx02  =  1/3; 

Wx2  =  zeros (n2 , 1 ) ; 
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%  Scaled  Sigma  Weights 
for  k  =  1 : n2 
if  k  ==  1 

wx2(k)=  1+ (Wx2 ( k) -1 ) /alpha^2 ; 

else 

wx2(k)  =  Wx2  ( k) /alpha''2  ; 

end 

end 


%  Incorperate  prior  PDF  knowledge  for  Higher  Order  Moments 
for  k  =  1 : n2 
if  k  ==  1 

wm2 ( k)  =  wx2 ( k) ; 

wc2(k)  =  wx2 (k) + (l-alpha^2+beta) ; 

else 

wm2 ( k)  =  wx2 ( k) ; 
wc2 ( k)  =  wx2 ( k) ; 

end 

end 


%  Sigma  Matrix 

sig2  =  zeros (nxa2,  n2); 

sig2 (1,1)  =  0 ; 

sig2(l,2)  =  -1/sqrt (2*wx2 (2) ) ; 
sig2(l,3)  =  1/sqrt (2*wx2 (2) ) ; 
for  j  =  2 : nxa2 
for  i  =  1:1 

sig2 ( j , i)  =  0; 

end 

for  i  =  2 : j  +1 

sig2(j,i)  =  -1/sqrt (j* (j+1) *wx2 (2) ) ; 

end 

for  i  =  j  +2 : j  +2 

sig2(j,i)  =  j /sqrt ( j * ( j+1) *wx2 (2) ) ; 

end 

end 


%  Pre-fill 
%  Initialize  (1,1) 
%  Initialize  (1,2) 
%  Initialize  (1,3) 


end 

%%  AHRS  ESTIMATOR 


%  Build  Augmented  State  Vector 

X  =  zeros (nxa,  n) ; 

xa  =  [x  bar;  zeros (nxa-nx, 1 )] ; 


%  Build  Augmented  Covariance  Matrix 
Sxa  =  [Sx  zeros (nx,nQ)  zeros (nx, nR) ; 

zeros (nQ,nx)  Qv  zeros (nQ, nR) ; 
zeros (nR,nx)  zeros (nR,nQ)  R] ; 


%  Build  n+2  Sigma  Vectors 
for  k  =  l:n 

X ( : , k)  =  xa  +  Sxa*sig ( : , k) ; 

end 
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%  Time  Update 
Xx  =  zeros (nx,  n) ; 

X  bar  =  zeros (nx,  1); 
for  k  =  1 : n 

%  Gyro  Model  w  bi (true)  =  w_bi (measured)  -  bias  -  noise 
w_bi_b  =  [p-X ( 5 , k) -X ( 8 , k) ;  q-X (6, k) -X (9, k) ;  r-X(7,  k)-X(10,k)] 

if  SIM  ==  1 

%  No  Sidereal  Rate 
w  bt  =  w  bi  b; 

else 

%  Sidereal  Rate  Included 
R_t2b  =  rot_t2b (X ( 1 : 4 , k) ) ; 

w_bt  =  w_bi_b  -  R_t2b* [wei*cos (latO) ;  0;  -wei*sin (latO) ] ; 

end 

%  Quaternion  Update 

wl  =  w  bt(l);  w2  =  w  bt(2);  w3  =  w  bt(3); 

Q  kpl  =  X(l:4,  k)  +  (dt/2)*[0,  -wl,  -w2,  -w3; 

wl,  0,  w3,  -w2; 

w2 ,  -w3,  0,  wl; 

w3,  w2 ,  -wl,  0]*X(1:4,  k) ; 

%  Quaternion  Normalization 
Q  kpl  n  =  Q  kpl/sqrt(Q  kpl'*Q  kpl); 

%  Gyro  Bias  Update 
if  SIM  ==  1 

%  Constant  Bias 
bg  =  X  ( 5  :  7 ,  k)  ; 

else 

%  Random  Walk  Bias 

bg  =  X(5:7,k)  +  dt*X(ll:13,k); 

end 

%  Rebuild  the  State  Vector  for  Each  Iteration 
Xx(:,k)  =  [Q_kpl_n;  bg] ; 

%  Calculated  the  Weighted  Mean  of  the  State  Vector 

X  bar  =  X  bar  +  wm ( k) *Xx ( : , k) ; 

end 

%  Calculate  Error  Covariance 

Ax  =  sqrt (wc (2 ) ) * (Xx ( : , 2 : n)  -  x_bar ( : , ones ( 1 , n-1 ) ) ) ; 

%  QR  Decomposision 
[~,  sx]  =  qr  (Ax'  ,  0)  ; 

%  Cholesky  Update  (Downdate  for  negative  zeroth  weight) 

Sx  =  cholupdate ( sx,  sqrt ( -wc ( 1 ) ) * (Xx ( : , 1 ) -x_bar ) ,  '-'); 

%  Non-Linear  Measurement  Process  Equations 

Y  =  zeros (nR,  n) ;  %  Pre-fill 
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y_bar  =  zeros (nR, 1 ) ; 

Heading  =  zeros (l,n); 
if  isempty(Hl) 

HI  =  zeros ( 1 , n) ; 

H2  =  zeros ( 1 , n) ; 

HeadingO  =  zeros (l,n); 
ii  =  zeros(l,n); 

end 

for  k  =  1 : n 

%  Rotation  Matrix  (LTP  to 
R  t2b  =  rot  t2b (Xx ( 1 : 4 , k) ) 

%  Gyro  Measurements 
w_bi  =  [p;  q;  r]  -  Xx(5:7, 
if  SIM  ==  1 

noise  R  =  X(ll;14,k); 
w  bt  =  w  bi; 

else 

noise  R  =  X(14:17,k); 
w_bt  =  w_bi  -  R_t2b* [wei*cos (latO) ;  0;  -wei*sin (latO) ] ; 

end 

%  Quaternion  to  Euler  Angles 

qO  =  Xx(l,k);  ql  =  Xx(2,k);  q2  =  Xx(3,k);  q3  =  Xx(4,k); 

psi  =  mod (atan2 (2* (ql*q2+q0*q3) ,  q0^2+ql^2-q2^2-q3^2) ,  2*pi)+ 

noise_R ( 4 ) ; 

%  Unwrap  Heading  Angle  From  [0,  2*pi]  Range  to  Aviod  Jumps 
if  nn  ==  1 

HI (k)  =  psi; 

HeadingO (k)  =  Hl(k); 

Heading (k)  =  Hl(k); 

else 

H2 (k)  =  psi; 

if  (H2 (k) -HI (k) )  <=  -100*pi/180 
ii(k)  =  ii(k)+l; 

end 

if  (H2 (k) -HI (k) )  >=  100*pi/180 
ii (k)  =  ii (k) -1; 

end 

Heading (k)  =  HeadingO (k) +( (H2 (k) +2*pi*ii (k) )  -  HeadingO (k)) 

HI (k)  =  H2 (k) ; 

end 

%  Accelerometer  Estimate 

FB_hat  =  R_t2b* [ax_gps;  ay_gps;  az_gps]  +  ... 

cross (w_bt,  R_t2b*x_bar2 (4 : 6) )  -  ... 

R_t2b*[0;  0;  g] -x_bar2 ( 7 : 9 ) -noise_R ( 1 : 3 ) ; 


%  Pre-fill 
%  Pre-fill 

%  Pre-fill 
%  Pre-fill 
%  Pre-fill 
%  Pre-fill 


Body) 


k)  ; 


%  Calculated  Observations 
Y ( : , k) = [FB_hat;  Heading ( k)]; 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm ( k) *Y ( ; , k) ; 
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end 


%  Estimated  Observation  Covariance 

Bx  =  sqrt (wc (2 ) ) * (Y ( : , 2 : n)  -  y_bar ( : , ones ( 1 , n-1 ) ) ) ; 

%  QR  Decomposition 

[~,  sy]  =  qr(Bx',0);  %  **"qr"  PRODUCES  UPPER  TRIANGULAR  MATRIX** 

%  Cholesky  Update  (or  Downdate) 

Sy  =  cholupdate ( sy,  sqrt ( -wc ( 1 ) ) * ( Y ( : , I ) -y_bar ) , 

%  ***Sy  MUST  BE  LOWER  TRIANGULAR*** 

Sy  =  Sy' ; 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx, nR) ; 
for  k  =  1 : n 

Pxy  =  Pxy  +  wc ( k) * ( (Xx ( : , k)  -  x  bar) * (Y ( : , k)  -  y  bar)'); 

end 

%  Measurement  Method  2  (Accelerometer) 

%  Heading  Unwrap  from  [0,  2*pi] 
if  nn  ==  1 

H3  =  heading; 

H4  =  0; 
psiO  =  H3; 
psi  m  =  H3; 

j  j=0; 

else 

H4  =  heading; 
if  (H4-H3)  <=  -100*pi/180 

jj  =  jj+1; 

end 

if  (H4-H3)  >=  100*pi/180 

jj  =  jj-1; 

end 

psi_m  =  psi0+ ( (H4+2*pi* j j )  -  psiO); 

H3  =  H4; 

end 

%  Measurement  Vector 
Z  =  [fx;  fy;  fz;  psi  m]  ; 

%  Kalman  Filter  Equations 
K  =  (Pxy/Sy' ) /Sy; 

Squares 

X  bar  =  X  bar  +  K* (Z  -  y  bar) ; 

%  Covariance  Corrections 
Ux  =  K*Sy; 
for  k  =  1 : nR 

Sx  =  cholupdate ( Sx,  Ux ( : ,  k)  ,  '-' )  ;  %  Cholesky  Downdate 

end 

Sx  =  Sx' ;  %  MUST  BE  LOWER 

TRIANGULAR 


%  Efficient  Least 
%  Kalman  Correction 
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nn 


=  2; 

%%  INS  ESTIMATOR 

%  Build  Augmented  State  Vector 

X  =  zeros (nxa2,  n2); 

xa  =  [x  bar2;  zeros (nxa2-nx2 , 1 )] ; 

%  Build  Augmented  Covariance  Matrix 

Sxa2  =  [Sx2  zeros (nx2 , nQ2 )  zeros (nx2 , nR2 ) ; 

zeros (nQ2 , nx2 )  Qv2  zeros (nQ2 , nR2 ) ; 
zeros (nR2 , nx2 )  zeros (nR2 , nQ2 )  R2 ]  ; 

%  Construct  n+2  SIGMA  Vectors 
for  k  =  1 : n2 

X(:,k)  =  xa  +  Sxa2*sig2 ( : , k) ; 

end 

%  Time  Update 

Xx  =  zeros (nx2,  n2);  %  Pre-fill 

X  bar2  =  zeros (nx2,  1);  %  Pre-fill 

for  k  =  1 : n2 

%  Rotation  Matrix 

R_t2b  =  rot_t2b (x_bar ( 1 : 4 ) ) ; 

R  b2t  =  R  t2b' ; 

%  Posittion  Update 

P_kpl  =  X(l:3,k)  +  dt*  [X  ( 4  :  5 ,  k)  ;  -X(6,k)]; 

%  Velocity  and  Bias  Update 
if  SIM  ==  1 
%  No  Sidereal 

V  kpl  =  X(4:6,k)  +  dt* (R  b2t* ( [fx;  fy;  fz]  -  X(7:9,k)  -  .. 

X(10:12,  k) )  +  [0;  0;  g] ) ; 
ba_kpl  =  X(7:9,k); 

else 

%  Sidereal  Included 

V  kpl  =  X(4:6,k)  +  dt* (R  b2t* ( [fx;  fy;  fz]  -  X(7:9,k)  -  .. 

X(10:12,  k) )  +  [0;  0;  g]  -  2*cross ( [wei*cos (latO) ; 
0;  -wei*sin (latO) ]  ,  X(4:6,k))); 
ba_kpl  =  X(7:9,k)  +  dt*X(13:15,k); 

end 

%  States 

Xx ( : , k)  =  [P  kpl;  V  kpl;  ba  kpl]; 

%  Calculated  Mean 

X  bar2  =  x  bar2  +  wm2 ( k) *Xx ( : , k) ; 
end 

%  Predicted  Error  Covariance 

Ax  =  sqrt (wc2 (2 ) ) * (Xx ( :  ,  2 : n2 )  -  x  bar2 ( : , ones ( 1 , n2-l ) ) ) ; 

%  QR  Decomposition 
[~,  Sx2]  =  qr  (Ax'  ,  0)  ; 

%  Cholesky  Factor  Update 
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Sx2  =  cholupdate ( Sx2  ,  sqrt ( -wc2 ( 1 ) ) * (Xx ( : , 1 ) -x_bar2 ) , 

%  Measurement  Equations 
Y  =  zeros (nR2,  n2); 
y_bar  =  zeros (nR2 , 1 ) ; 
for  k  =  1 : n2 

if  SIM  ==  1 

%  Calculated  Observations 
Y(;,k)=Xx(l:6,k)+X(13:18,k)  ; 

else 

%  Calculated  Observations 
Y  ( : ,  k)  =Xx  (1 :  6,  k)  +X  ( 1 6  :  2 1 ,  k)  ; 

end 

%  Calculated  Observation  Mean 
y  bar  =  y  bar  +  wm2 ( k) *Y ( : , k) ; 
end 

%  Estimated  Observation  Covariance 
Bx  =  sqrt (wc2 (2 ) ) * (Y ( :  ,  2 : n2 )  -  y_bar ( : , ones ( 1 , n2-l ) ) ) ; 

%  QR  Decomposition 
[~,  Sy]  =  qr (Bx' , 0) ; 

%  Cholesky  Factor  Update 

Sy  =  cholupdate ( Sy,  sqrt ( -wc2 ( 1 ) ) * ( Y ( : , 1 ) -y_bar ) , 

%  Make  Sy  Lower  Triangular 
Sy  =  Sy' ; 

%  Estimated  Cross  Covariance 
Pxy  =  zeros (nx2  ,  nR2 )  ; 
for  k  =  1 : n2 

Pxy  =  Pxy  +  wc2 ( k) * ( (Xx ( : , k)  -  x  bar2 ) * ( Y ( : , k)  -  y  bar)'); 

end 

%  Measurements 

Z  =  [N_sat;  E_sat;  D_sat;  speed*cos (CoG) ;  speed*sin (CoG)  ;  Vd_sat] 

%  Kalman  Filter  Equations 
%  Kalman  Gain  w/  Efficient  Least  Squares 
K  =  (Pxy/Sy' ) /Sy; 

%  Kalman  State  Corrections 
X  bar2  =  x  bar2  +  K* (Z  -  y  bar)  ; 

%  Covariance  Correction 
Ux  =  K*Sy; 
for  k  =  1 : nR2 

Sx2  =  cholupdate ( Sx2 ,  Ux ( : , k) ,  '-' ) ; 

end 

%  Make  Sx2  Lower  Right  Trangular 
Sx2  =  Sx2' ; 

X  HAT  =  [x  bar (1:4);  x  bar2(l:6);  x  bar (5: 7);  x  bar2(7:9)]; 
end 

kk=kk+l; 

end 


%  Pre-fill 
%  Pre-fill 
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function  R  t2b  =  rot  t2b(x) 

qO  =  x(l);  ql  =  x(2);  q2  =  x(3);  q3  =  x(4); 

R_t2b  =  [q0^2  +  ql''2-q2^2-q3''2  2*  (ql*q2  +  q0*q3)  2*(ql*q3-q0*q2); 

2* (q2*ql-q0*q3)  qO ^2 -ql ^2+q2 ^2 -q3 ^2  2*(q2*q3+q0*ql); 

2*  (q3*ql  +  q0*q2)  2  *  ( q3  *q2 -qO  *ql )  qO  ^2 -ql '^2 -q2  ^2  +  q3 -^2  ]  ; 

end 

function  q  =  e2q(phi,  theta,  psi) 

%  Form  Rotation  Matrix 

R_psi  =  [cos (psi)  sin (psi)  0;  -sin (psi)  cos (psi)  0;  0  0  1]; 

R_theta  =  [cos (theta)  0  -sin  (theta) ;  0  1  0;  sin  (theta)  0  cos (theta) ] 
R_phi  =[10  0;  0  cos (phi)  sin (phi);  0  -sin (phi)  cos  (phi) ]; 

R  n2b  =  R  phi*R  theta*R  psi; 

%  Extract  Quaternions 
q0=sqrt (1+trace (R_n2b) ) /2; 
ql= (R_n2b (2,3) -R_n2b (3,2) ) / (4*q0) ; 
q2= (R_n2b (3, 1) -R_n2b (1, 3) ) / (4*q0) ; 
q3= (R_n2b (1,2) -R_n2b (2, 1) ) / (4*q0)  ; 

q  =  [qO;  ql;  q2 ;  q3]; 
end 
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